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none  of  these  capabilities,  they  simply  grasp  objects  rigidly  in  their  end  effec¬ 
tors.  To  investigate  this  common  form  of  human  manipulation,  an  analysis  of 
controlled  slipping  of  a  grasped  object  within  a  robot  hand  was  performed. 
The  Salisbury  robot  hand  demonstrated  many  of  these  controlled  slipping 
techniques,  illustrating  many  results  of  this  analysis. 
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First,  the  possible  slipping  motions  were  found  as  a  function  of  the  lo¬ 
cation,  orientation,  and  types  of  contact  between  the  hand  and  the  object. 
Second,  for  a  given  grasp,  the  contact  t3rpes  were  determined  as  a  function  of 
the  grasping  force  and  the  external  forces  on  the  object.  Finally,  by  chang¬ 
ing  the  grasping  force,  the  robot  modified  the  constraints  on  the  object  and 
affect  controlled  slipping  motions. 
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Chapter  1  ^ 

Introduction 


1.1  Introduction 

When  we  manipulate  objects,  our  fingers  are  not  always  fixed  to  the  surface. 
Many  times  we  allow  the  objects  we  hold  to  slide  or  rotate  at  our  fingertips, 
consciously  controlling  the  motion  of  the  object  rather  than  the  motion  of 
our  fingers.  This  controlled  slipping  technique  of  manipulation  is  not  jtxst  one 
of  the  ways  we  can  move  objects,  but  rather  a  dominant  form  of  dexterous 
human  manipulation.  For  example,  try  putting  a  lid  on  a  jar,  but  start  with 
the  lid  top  down  on  a  table.  Without  thinking,  we  pick  up  the  lid,  spin  it 
around  between  our  two  fingers,  using  the  edge  of  the  jar  or  another  finger, 
and  screw  it  on  the  top.  Or  consider  the  use  of  a  pencil  eraser.  When  we 
make  a  mistake  we  stop  writing,  flip  the  pencil  over,  push  the  pencil  through 
our  fingers,  and  erase.  In  both  these  examples  and  in  many  others,  we  allow 
objects  to  slide  and  rotate  at  our  fingertips.  Through  this  controlled  slipping 
technique  we  can  control  the  location  and  orientation  of  an  object  within  our 
graisp. 

In  robotic  manipulation,  emphasis  has  been  placed  on  producing  stable 
grasps.  The  object  is  then  moved  by  controlling  the  motion  of  the  manipula¬ 
tor,  assuming  the  object  is  rigidly  fixed  to  the  robot.  If  the  object  slips  in  the 
grasp,  control  is  lost,  and  there  are  no  easy  forms  of  recovery.  In  addition, 
there  are  many  operations  which  become  difficult  or  impossible  without  some 
form  of  controlled  slip  manipulation.  In  this  thesis,  I  will  analyze  controlled 
slip  manipulation  in  a  robot  hand,  and  from  this  analysis  predict  slipping 
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motion  of  an  object  within  a  grasp.  Using  these  predicted  slipping  motions, 
the  robot  hand  will  affect  the  necessary  changes  in  the  grzisp  to  allow  the 
object  to  slip  in  a  controlled  manner. 


1.2  Overview 

In  order  for  a  multifingered  robot  hand  to  perform  dexterous  operations 
on  the  environment,  it  must  be  able  to  acquire  objects  into  a  grasp,  con¬ 
trol  the  motion  of  the  object  relative  to  the  environment  and  control  the 
object  within  the  grasp.  Object  acquisition  is  an  active  area  of  robotic  re¬ 
search.  Given  a  particular  manipulator  and  an  object,  how  can  the  robot 
grasp  the  object?  There  are  numerous  considerations,  such  as  the  location, 
size,  weight,  orientation,  surface  properties,  and  specific  functions  of  the  ob¬ 
ject,  as  well  as,  configuration,  workspace,  strength,  and  surface  properties  of 
the  manipulator.  An  algorithm  recently  developed  [Nguyuan],  computes  the 
grasp  locations  on  an  object  and  finger  stiffnesses  in  the  robot  necessary  to 
produce  force  closure  grasps.  The  objects  are  modeled  as  polyhedrons  with 
a  certain  mass  and  surface  friction.  Areas  on  the  polyhedral  surfaces  are 
then  found  on  which  the  fingertips  of  a  multifingered  hand  may  be  placed 
to  yield  force  closure  grasps.  The  number  of  fingers,  the  surface  properties 
of  the  fingertips,  the  stiffness  of  the  joints,  and  the  workspace  of  the  hand 
are  all  taken  into  account  when  computing  grasp  locations.  Some  research  in 
object  acquisition  has  also  been  done  by  [Lozano-Perez].  Not  only  were  the 
current  constraints  on  the  object  considered,  but  also  constraints  imposed  on 
the  object  by  the  environment,  as  the  object  is  moved  through  its  planned 
trajectory.  The  constraints  both  present  and  future  are  mapped  onto  the 
object  surface  before  the  object  is  grasped.  In  this  way,  for  example,  a  robot 
gripper  would  not  grab  the  end  of  the  peg  which  later  must  be  inserted  into 
a  hole.  There  are,  however,  still  many  areas  of  object  acquisition  not  yet 
explored.  Objects  with  special  surfaces,  such  as  handles  or  loops,  non-rigid 
objects,  like  paper  or  foam,  objects  that  must  be  moved  before  grasping,  such 
as  a  coin  on  a  flat  table,  are  all  common  examples  acquisition  which  are  not 
yet  possible  for  a  robot. 

The  control  of  object  motion  through  the  coordinated  control  of  individ¬ 
ual  manipulators  is  also  a  current  area  of  robot  research.  An  algorithm  has 
recently  been  written  [Chiu]  which  coordinates  individual  fingers  to  yield  a 
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specific  translations  and  rotations  of  a  grasp  relative  to  an  arbitrary  refer¬ 
ence  frame.  The  object  is  etssumed  to  remain  fixed  relative  to  the  grasp; 
therefore,  specific  motions  of  the  grasp  yield  specific  motions  of  the  object. 
From  practical  experience,  this  method  has  worked  quite  well.  In  order  to 
actually  control  the  motion  of  the  object,  however,  it  would  be  necessary  to 
sense  the  position  and  orientation  of  the  object,  since  the  object  may  have 
been  removed,  slipped,  or  fallen  from  the  greisp,  unknown  to  the  robot.  Real 
time  hand-eye  coordination,  however,  is  beyond  the  reach  of  current  systems 
because  of  the  computational  complexity.  High  level  coordinated  control  of 
multifingered  robot  hands,  is  a  relatively  new  area  of  research.  Recent  mul¬ 
tifingered  robot  hand  research,  therefore,  has  been  primarily  concerned  with 
mechanical  design,  actuation,  transmission,  and  sensing. 

Control  of  an  object  relative  to  a  grasp  is  also  a  new  area  of  research. 
This  problem  has  be  approached  by  [Toumaissoud],  in  terms  of  regrzisping. 
The  object  is  initially  graisped,  then  set  down,  released,  and  then  regrasped. 
In  this  way  the  object  can  be  reoriented  relative  to  the  grasped.  The  object, 
however,  is  not  manipulated  within  the  grasp.  Controlled  slip  manipulation, 
however,  could  enhance  the  dexterity  of  a  manipulator,  by  allowing  the  robot 
greater  freedom  to  move  the  object  within  the  grasp. 


1.3  Outline 

This  thesis  is  basically  an  analysis  of  the  small  finite  permissible  motions 
an  object  may  undergo  in  a  particular  grasp  and  how  these  motions  may 
be  achieved.  The  first  chapter  is  the  introduction.  In  chapter  2,  the  types 
of  contacts  between  a  fingertip  and  an  object  are  enumerated.  For  each 
set  of  contacts  and  contact  types,  we  define  a  constraint  state  and  for  each 
constraint  state,  a  map  of  the  possible  object  motions  is  produced.  Deter¬ 
mining  the  constraint  on  small  finite  motion,  requires  knowing  not  only  is 
the  location  and  orientation  of  contacts,  but  also  the  surface  geometry  in  the 
neighborhood  of  the  contact.  Chapter  3  deals  with  strategies  available  to  ma¬ 
nipulate  an  object  relative  to  a  grasp.  These  include  the  use  of  gravity,  body 
forces,  controlled  accelerations,  free  fingers,  and  other  objects  to  move  objects 
within  the  grasp.  For  a  multifingered  hand  there  are  many  ways  to  squeeze  a 
grasped  object.  For  two  or  three  fingered  grasps,  there  is  corresponding  one 
space  or  three  space  of  possible  grasp  solutions.  Chapter  4  presents  a  sim- 
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pie  intuitive  parameterization  of  this  grasp  force  space.  For  a  three  fingered 
hand,  the  three  space  may  be  represented  simply  by  a  grasp  force  focus  and  a 
grasp  force  magnitude.  In  order  to  determine  the  type  of  contact  that  exists 
at  each  fingertip,  the  forces  and  moments  transmitted  through  the  interface 
must  be  found.  In  chapter  5,  the  set  of  forces  and  moments  that  exist  at  the 
contacts  will  be  found  in  terms  of  the  contact  frame,  that  is,  a  coordinate 
frame  set  in  the  object  and  defined  in  terms  of  the  contact  point  and  outward 
pointing  normal.  Chapter  6  outlines  a  simple  relationship  between  the  forces 
and  moments  at  the  contact  and  the  contact  type.  Two  examples  are  given 
in  chapter  7  illustrating  the  analyses  of  the  previous  chapters,  along  with  an 
demonstration  of  how  controlled  slipping  could  be  used  to  reorient  an  object 
within  a  grasp.  Chapter  8  describes  how  these  analyses  were  simulated  and 
how  some  controlled  slip  manipulations  were  achieved  on  the  Salisbury  robot 
hand.  In  chapter  9,  extensions  of  the  present  theories  are  outlined,  which 
yield  more  practical  and  efficient  techniques  for  controlled  slip  manipulation. 


Chapter  2 


Constraint 


2.1  Introduction 

The  purpose  of  this  chapter  is  to  determine  the  different  ways  an  object  can 
move  in  a  grasp.  First,  the  screw  system  representation  will  be  described. 
This  representation  allows  a  complete  and  homogeneous  treatment  of  both 
forces  and  moments  as  well  as  translation  and  angular  displacements.  Sec¬ 
ond,  the  types  of  contacts  which  may  occur  between  two  objects  will  be 
enumerated.  For  each  contact  type,  a  particular  set  of  forces  and  moments 
can  be  exerted  through  the  interface.  These  forces  and  moments  limit  the 
possible  motions  of  one  object  relative  to  another.  Third,  the  set  of  infinites¬ 
imal  motions  possible  for  an  object  subject  to  a  single  constraint  will  be 
determined  using  the  concept  of  virtual  work.  Fourth,  the  set  of  infinitesi¬ 
mal  motions  will  be  extended  to  include  the  set  of  small  finite  motions  by 
considering  the  surface  geometry  in  the  neighborhood  of  the  contact  point. 
Fifth,  a  constraint  state  will  be  defined  as  an  ordered  list  of  contact  types  on 
the  object.  Finally,  for  each  constraint  state,  the  set  of  permissible  motions 
for  a  grasped  object  will  be  found  by  intersecting  the  permissible  motions  of 
the  individual  contacts. 


2.2  Screws,  wrenches,  and  twists 

Many  of  the  analyses  in  this  thesis  will  employ  a  screw  system  representa¬ 
tion  for  forces  and  moments,  and  for  infinitesimal  displacements.  Although 
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standard  force  and  displacement  vectors  could  be  used,  screw  systems  allow 
a  homogeneous  treatment  of  both  forces  and  moments,  and  translational  and 
angular  displacements. 

A  set  of  forces  and  moments  acting  upon  a  body  can  be  collectively  called 
a  wrench.  A  wrench  may  be  described  as  a  force  along  a  unique  line,  the 
screw  axis,  and  a  moment  about  that  line.  Similarly,  a  twist  can  represent 
the  infinitesimal  motion  of  an  object^  an  infinitesimal  translation  along  a  line 
and  an  infinitesimal  rotation  about  that  line. 

2.2.1  Screws 

The  outline  of  screw  systems  presented  here  is  more  adequately  described  in 
[Hunt]  zmd  completely  developed  in  [Ball].  Both  the  wrench  and  the  twist 
are  specific  representations  of  a  screw.  A  screw  is  defined  by  a  line  in  three 
space,  a  screw  axis,  and  an  associated  pitch  about  that  line.  A  screw  may 
also  be  described  by  a  six  element  vector,  s  =  ["31,32,33,34,35,56],  where 
3i,32,  ...,36  are  the  screw  coordinates.  The  coordinates  of  the  screw  axis 
are 

I  =  5i 
M  =  S2 
AT  =  53 

P  =  54-p5i  (2.1) 

Q  =  Si-pS2 
R  =  Se  —  pSj. 


The  coordinates  L,M, , . .  ,R  are  known  as  the  Plucker  line  coordinates  of 
the  axis,  where  L,M,  and  N  are  proportional  to  the  direction  cosines  of  the 
screw  axis,  and  P,Q,  and  R  are  proportional  to  the  moment  of  the  line  about 
the  origin  of  the  reference  frame.  The  pitch  of  the  screw  is 

S1S4  +  SzSs  +  SsSe 
+  +  ’ 

and  the  magnitude  of  the  screw  is 


(2.2) 
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unless  the  pitch  happens  to  be  infinite,  in  which  case  the  magnitude  of  the 
screw  is  _ 


m  =  yJSl  +  Si  +  Si 


(2.4) 


2.2.2  Wrenches 

The  wrench  is  one  interpretation  of  the  screw  and,  therefore,  may  be  defined 
in  a  similar  way.  The  wrench  may  be  identified,  in  terms  of  screw  coordinates, 
by  a  six  element  vector  w  =  [lOi ,  tuj,  tt>3,  W4,  Ws,  tys]?  where  Wj  ,W2,  and  W3  are 
the  forces  along  the  x,y,  and  z  axes  of  an  reference  frame,  and  104,1^5,  and  We 
are  the  moments  about  the  axes  of  the  reference  frame.  By  replacing  Si  with 
Wi  in  equation  2.1,  the  line  coordinates  of  the  wrench  may  be  found, 

L  wx 
M  =  w-x 
N  =  W3 

P  ss  W4  —  pwl  (2.5) 

Q  sz  Wi  —  pwx 
R  =  iwe  —  jnvs' 


The  pitch  of  the  wrench  is  given  by  equation  2.2, 


_  wxW4  +  wjWj  -I-  WsWe 
^  Wi  A  W2 

and  is  the  ratio  of  the  torque  about  the  screw  axis  to  force  along  it. 
magnitude  of  the  wrench  from  equations  2.3  or  2.4  is 

m  =  y/wf  +  1^2  +  wl 

or  if  the  pitch  is  infinite,  the  magnitude  is 


(2.6) 

The 


(2.7) 


m 


(2.8) 


2.2.3  Twists 

As  with  the  wrench,  the  twist  can  also  be  described  in  terms  of  a  six  element 
vector  t  =  [^1,  tj,  <3,  ^4,  <5,  where  <1,^2,  and  are  the  rotations  about  the 
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X,  y,  and  z  axes  of  a  reference  frame  and  <4, <5,  and  are  the  displacements 
along  the  x,y,  and  z  axes.  Again,  replacing  S,-  with  ti,  the  line  coordinates 
of  the  twist  are 

L  — 

A/  =  ^2 

N  =  h 

P  =  <4-p<i  (2.9) 

Q  =  ts-  ph 

R  =  tg  —  ptz. 

The  pitch  of  the  twist  is 


_  ^1^4  d"  ^2^5  +  ^3^6 


(2.10) 


that  is,  the  ratio  of  the  rotation  about  the  twist  axis  to  the  translational 
along  the  axis.  The  magnitude  is  defined  by 


m 


(2.11) 


or  in  the  case  where  the  pitch  is  infinite,  the  magnitude  is 

m  -  i/ff+lf+ff. 


(2.12) 


2.2.4  Transforming  screw  definitions 

Although  a  wrench  and  a  twist  are  independent  of  the  reference  frames  used 
to  define  them,  it  is  useful  to  be  able  to  transform  their  representations 
from  one  frame  to  another.  In  general,  a  screw  s'  can  be  represented  in 
screw  coordinates,  s'  =  [51,32,33, 34,35,  Sg],  where  the  elements  of  s'  are 
defined  relative  to  a  specific  reference  frame.  Let  the  elements  of  the  screw 
s'  be  defined  relative  to  a  specific  coordinate  frame  O'X'Y'Z'.  Suppose  this 
reference  frame  is,  in  turn,  defined  with  respect  to  another  reference  frame 
OXYZ^  figure  2.1.  That  is,  let  1  =  [/*,/,,/*],  m  =  [m*,my,  m,],  and  n  = 
[nr,ny,  nj],  be  the  unit  direction  vectors  of  the  x',  y',  and  z'  axes  and  let 
X  =  [x,y,z]  be  the  origin  of  the  O'X'Y'Z'  frame  all  defined  with  respect 
to  the  OXYZ  frame.  Suppose  we  now  wish  to  define  the  screw  s'  in  screw 
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coordinates  "elative  to  the  OXYZ  frame.  Let  s  be  the  representation  of  the 
screw  in  the  OXY Z  frame,  then 


s  =  Ts', 

where  T  is  a  linear  transformation  matrix  given  by 


(2.13) 


T  = 


/x 

L/z 


TTlj 

m. 


m. 


n* 

n„ 


n 


*  J 


Lz  —  hx 


in  O'X'Y'Z', 
where  T~^  equals 


T-i  ^ 


Ly  -  lyZ 
m^y  —  rriyZ 
n^y  -  UyZ 


zy  - 

TTtyZ 

Tizy 

—  n. 

cZ  - 

m^x 

rixZ 

—  fi: 

,x- 

mj.y 

TlyX 

—  n. 

defined  in 

the  0X1 

s' 

=  T 

1. 

iy 

/x  ■ 

m* 

my 

m. 

«x 

Tty 

. 

/x 

z  -  lz 

X 

lyX 

rrij 

m. 


Tlx 

n*. 


rij 


(2.14) 


(2.15) 


rrij^z  —  m^x  rriyX  — 

n*z  —  n^x  riyX  —  Thy  J 


/x 

rux 

n* 


my 

Tl*/ 


n. 


Since  the  twist  is  a  specific  representation  of  a  screw,  it  too  can  be  defined 
in  screw  coordinates  relative  to  different  reference  frames.  Suppose  a  twist 
t'  is  defined  relative  to  a  frame  O'XY'Z'.  Again,  suppose  this  frame  is,  in 
turn,  defined  with  respect  to  another  frame  OXY Z.  The  representation  t  of 
the  twist  t'  in  the  frame  OXYZ  is  given  by 


t  =  Tt', 

where  T  is  given  in  equation  2.14  and  conversely, 

t'  =  t-h, 


(2.17) 


(2.18) 
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Figtire  2.1:  Although  the  screw  is  the  same  no  matter  which  reference  frame  is 
used  to  define  it,  it  is  helpful  to  be  able  to  transform  the  representation  from  one 
frame  to  another.  Suppose  a  screw  s'  is  defined  in  screw  coordinates  relative  to 
a  reference  frame  0‘X'Y'Z'  and  O'X'Y'Z'  is,  in  turn,  defined  relative  to  another 
frame  OXYZ.  Then  the  screw  can  also  be  represented  in  the  OXYZ  frame  by  a 
simple  linear  transformation  s  s  Ts'. 

where  T“^  is  given  in  equation  2.16.  Similarly  for  the  wrench,  given  in  screw 
coordinates  relative  to  O'X'Y'Z',  its  representation  in  OXY  Z  is  given  by 

w  =  TV,  (2.19) 

where  T  is  given  in  equation  2.14.  Conversely,  the  wrench  defined  in  the 
OXY Z  frame  may  also  be  represented  in  the  O'X'Y'Z'  by 

w'  =  T- V,  (2.20) 

where  T~^  is  given  in  equation  2.16. 

2.3  Contact  types 

The  interface  between  a  robot  and  an  object  can  be  characterized  by  a  par¬ 
ticular  type  of  contact.  For  a  three  dimensional  manipulator,  there  are,  in 
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general,  nine  different  types  of  contacts.  These  contact  types  are  illustrated 
in  figure  2.2,  [Salisbury].  For  each  of  these  contact  types,  a  particular  set 
of  wrenches  may  be  exerted  through  the  interface.  The  set  of  all  possible 
wrenches  that  may  be  transmitted  through  a  contact  is  known  as  a  wrench 
system.  A  set  of  unit  basis  wrenches  can  be  specified  which  span  the  entire 
wrench  system.  The  nature  of  the  contact  problem,  however,  is  inherently 
non-linear,  since  objects  can  both  make  and  break  contact  with  one  another. 
Therefore,  to  describe  the  wrench  system,  it  is  necessary  to  define  a  set  of 
unidirectional  unit  basis  wrenches.  Any  wrench  in  the  wrench  system  may 
then  be  represented  by  a  positive  linear  combination  of  these  unidirectional 
basis  wrenches.  Figure  2.2  lists,  along  with  each  contact  type,  the  set  of 
unidirectional  unit  basis  wrenches  whose  positive  linear  combinations  span 
the  space  of  all  the  possible  wrenches  which  can  be  transmitted  through  the 
contact. 

For  the  analyses  in  this  thesis,  only  contacts  made  between  the  fingertips 
of  the  manipulator  and  an  object  will  be  considered,  although  the  techniques 
developed  here  encompass  other  types  of  contacts  as  well.  By  considering 
only  fingertip  contacts,  however,  the  number  of  possible  contact  types  is 
reduced  to  four:  a  soft  finger  contact,  a  point  contact  with  friction,  a  point 
contact  without  friction,  and  no  contact.  For  convenience,  these  contact 
types  will  be  represented  by  the  following  numbers 

1  =  Soft  finger  contact 

2  =  Point  contact  with  friction 

3  =  Point  contact  without  friction 

4  =  No  contact. 


2.4  Infinitesimal  motion 

Virtual  work  is  the  work  done  by  a  wrench  exerted  against  an  arbitrary 
twist.  By  examining  the  sign  of  the  virtual  work,  we  can  determine  the  set 
of  infinitesimal  motions  which  are  possible  for  an  object  constrained  by  a 
contact.  First  a  contact  is  characterized  by  a  particular  contact  type.  Then 
the  contact  is  replaced  by  a  set  of  unidirectional  unit  basis  wrenches,  listed 
in  the  figures  2.2  and  2.3.  Then  for  each  basis  wrench,  the  virtual  work 
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Unit  Basis  Wrenches 


(1,0.0, 0,0,0)  u;  =  (-l. 0,0,0, 0,0) 
[0,1, 0,0, 0,0]  w  =  [0,-1, 0,0, 0,0) 
u;  =  [0,0,-l. 0,0,0) 
[0,0,0, 1,0,0]  u>  =  [0,0,0, -1,0,0] 

[0,0,0, 0,1,0)  ti)  =  [0,0, 0,0, -1.0) 

[0,0,0,0,0,11  w  =  [0,0, 0,0,0, -ij 


Line  contact  with  friction 


u>  =  [1,0,0.0,0,01 
u>  =  [0,1, 0.0, 0,0] 

w  =  [0,0,0, 1,0,0) 
w  =  [0,0, 0.0, 0,1) 


u,  =  (1,0,0.0,0.01 
u>  =  [0,1,0,0,0,01 


tc  =  [0,0, 0,0, 0,1] 


w*  [1,0.0,0,0,01 

w  =  [0.0,1,0,0,01 
w  =  [0,0,0, 1,0,0) 
w  =  [0,0,0,0,1,01 


W  =  [-1,0,0,0.0,01 
W  =  (0,-1, 0,0, 0.0] 
w  =  [0, 0,-1, 0.0,0] 

W  =  [0,0, 0,-1, 0,0) 
U>  =  [0,0.0,0.0, -1) 


w  =  [-1,0,0,0.0.01 
w  =  [0,-1, 0,0, 0,0) 
u;«  10,0,-1.0,0,01 


w  =  [0,0,0,0,0,  -ii 


w  =  [-1,0,0,0,0,01 
«>  =  [0,-l,0, 0.0,0) 
w  =  [0,0.-l,0,0,01 
u;  =  [0,0, 0,-1, 0.0) 
(0,0, 0,0, -1,0) 


Plane  contact  without  friction 

Figure  2.2:  In  general,  there  are  nine  different  types  of  three  dimensional  con¬ 
tacts.  Each  contact  type  allows  only  certain  wrenches  to  be  transmitted  through 
the  interface.  Since  the  contact  problem  is  inherently  non-linear,  a  set  of  unidi¬ 
rectional  basis  wrenches  is  defined  whose  positive  linear  combination  describe  the 
space  of  wrenches  which  can  be  transmitted  through  the  interface. 
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Point  contact  with  friction 


Line  contact  without  friction 


Unit  Basis  Wrenches 


w  =  [l,0,0.0,0.0j  tv  =  [-l,0,0,0,0,0j 
w  =  [0,1,0.0.0,01  w  =  [0. -1,0, 0,0,0] 
w  =  [0.0,-1.0,0,0j 


w  =  [0,0.-l. 0,0.0] 

w  =  {0,0,0,1.0,01  u;  =  [0,0,0, -1,0.0] 


Point  contact  without  friction 


w  =  [0,0, -1,0, 0.0] 


Figure  2.3:  Contact  types  (Continued) 
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is  calculated.  If  the  virtual  work  done  by  a  twist  against  every  unit  basis 
wrench  in  the  set  is  greater  than  or  equal  to  zero,  then  the  twist  is  allowed. 
Conversely,  twists  which  produce  negative  virtual  work  for  even  a  single  basis 
wrench  are  disallowed. 

A  complete  derivation  of  virtual  work  is  presented  in  [Ball]  and  the  deriva¬ 
tions  in  this  section  are  outlined  in  [Ohwovoriole].  A  robot  manipulator  A 
exerts  a  wrench  of  magnitude  a  and  pitch  Pa  along  a  screw  A  on  a  body  B, 
as  shown  in  figure  2.4.  The  body  then  undergoes  a  twist  of  amplitude  /?  and 
pitch  p0  along  a  screw  B.  Define  a  coordinate  system  OXY Z  with  the  x-axis 
aligned  with  the  twist  axis,  the  z-axis  the  common  perpendicular  between 
the  twist  and  wrench  axes,  and  the  y-axis  perpendicular  to  both  the  x  and  z 
axes,  as  shown  in  figure  2.5.  The  wrench  can  then  be  decomposed  into  forces 
and  moments  on  the  cartesian  coordinate  system 

Fx  =  oc  cos  9 
Fy  —  a  sin  9 
Fx  =  0 

A/*  =  a(Pa  cos  ^  sin  0)  (2-21) 

My  =  a{pa  sin  9  A  d  cos  9) 

A/,  =  0, 

where  d  is  the  length  of  the  mutual  perpendicular  between  the  wrench  and 
the  twist  axes.  The  virtual  work  done  by  the  wrench  against  the  twist  is 

W  =  a/3  cos[(pa  +  pp)  cos 9  —  dsin  9],  (2.22) 

The  virtual  coefficient  between  screws  A  and  B  is  defined  to  be 

w  =  cos[(po -f  p^)  cos d  —  dsin 0).  (2.23) 

The  virtual  coefficient  is  independent  of  both  the  amplitude  of  the  twist  and 
the  intensity  of  the  wrench.  Let  (<i,  <2,  ta,  <4,  ts,  <«)  and  (uji,  lyj,  1^4,  ws,  ws) 
be  the  screw  coordinates  of  a  unit  twist  and  a  unit  wrench.  The  virtual 
coefficient  is  then 


u}  =  Wit^  -I-  W2t$  4-  +  Wstj  +  weti.  (2.24) 

We  will  use  the  unit  twist  and  unit  twist  from  now  on  when  evaluating 
the  virtual  coefficient.  The  reason  for  this  will  be  apparent  later  when  we 
consider  the  value  of  the  virtual  coefficient. 
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Figure  2.4:  The  contact  forces  between  a  manipulator  A  and  a  body  B  can  be 
modeled  as  a  wrench  of  w  along  a  screw  A  of  magnitude  a  and  pitch  pa.  The 
motion  of  the  body  can  be  described  by  a  twist  t  along  a  screw  B  of  amplitude  /9 
and  pitch 


Figure  2.5:  The  wrench  can  be  decomposed  onto  a  coordinate  system  OXYZ. 
The  z-axis  is  common  perpendicular  between  the  twist  and  the  wrench  axes,  the 
x-axis  is  the  twist  axis,  and  the  z-axis  is  perpendicular  to  both  the  x  and  the  z 


axes. 


CHAPTER  2.  CONSTRAINT 


16 


2.4.1  Reciprocal  screws 

Two  screws  are  said  to  be  reciprocal  screws  if  the  virtual  work  between  a 
twist  and  a  wrench  associated  with  the  two  screws  is  zero.  No  work  will  be 
done  by  a  unit  twist  t  against  a  unit  wrench  w  if  the  virtual  coefficient  is 
equal  to  zero.  That  is, 


uj  =  Wit4  +  w-iU  +  =  0.  (2.25) 


2.4.2  Repelling  screws 

Two  screws  are  said  to  be  repelling  if  the  virtual  work  between  the  screws  of 
a  twist  and  a  wrench  is  positive.  Therefore  two  unit  screws  of  a  unit  twist 
and  a  unit  wrench  will  repelling  if  the  virtual  coefficient  if  positive.  That  is, 

uj  =  wxt4  +  +  ^aU  +  >  0-  (2.26) 

Work  is  defined  to  be  positive  if  the  body  moves  in  the  same  direction  as  the 
wrench. 


2.4.3  Contrary  screws 

Two  screws  are  contrary  if  the  virtual  work  between  the  screws  of  a  twist 
and  a  wrench  is  negative.  Again  two  unit  screws  are  contrary  if  the  virtual 
coefficient  if  negative. 


w  =  voxt4  +  +  tuai®  +  fJOA^i  "b  *^5^3  "b  ^  0*  (2.27) 


2.4.4  Permissible  twist 

A  permissible  twist  is  an  infinitesimal  motion  that  the  constrained  object  is 
allowed  to  undergo.  Assxxme  an  object  is  constrained  by  a  single  contact,  as 
shown  in  figure  2.6.  Also  assume  that  the  contact  can  be  characterized  by 
a  particular  contact  type.  The  constraint  imposed  by  the  contact  can  then 
be  represented  by  its  associated  set  of  unidirectional  unit  bases  wrenches  as 
was  described  earlier. 

For  convenience,  set  of  bases  wrenches  is  defined  in  terms  of  a  contact 
frame.  The  contact  frame  is  a  coordinate  frame  whose  origin  is  the  point  of 
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contact  between  the  object  and  the  manipulator.  The  z-axis  is  the  surface 
normal  at  the  contact  point,  and  the  x  and  y  axes  lie  in  the  tangent  plane. 
In  general,  the  choice  of  the  x  and  y  axes  of  the  contact  frame  is  arbitrary; 
however,  a  convention  for  resolving  the  ambiguity  is  given  in  appendix  A. 
This  convention,  though  useful  for  the  implementation,  is  unimportant  in 
the  current  discussion.  Figure  2.6  then  shows  the  contact  frame  Oc< 
defined  relative  to  a  reference  frame  OXY Z. 

Let  {wcj}  be  the  set  of  unidirectional  unit  bases  wrenches  which  replace 
the  contact  type  defined  in  terms  of  the  contact  frame.  Their  representation 
in  the  reference  frame  OXY Z  is  given  by 

Wj  =  TiWcj,  (2.28) 

where  Ti  is  the  transformation  matrix  given  in  equation  2.14  by 

\y  TTly  Tly  0 

Ig  TTig  n,  J 

Uy  -  lyZ  '^zy  —  rriyZ  n^y  -  riyZ  1  f  m;^  n,,  I  ’ 

/x'2'  IgX  *  TTlg^S  TtjgZ  Ttg^  ly  Tfty  Tly 

lyX  —  lg.y  TTiyy  —  rtig-y  rtyX  —  i  h  ng  j  \  . 

(2.29) 

where  1  =  [/*,  /y,  /,],  m  =  [m*,  m^,  m,],  and  n  =  [n^,  n^,  n*]  are  the  direction 
vectors  of  the  x,y,  and  z  axes  and  x,  y,  and  z  is  the  origin  of  the  contact 
frame  defined  with  respect  to  the  reference  frame  OXYZ. 

The  set  of  twists  which  are  reciprocal  or  repelling  to  a  single  unit  basis 
wrench  wj  is 

Tj  ~  {t :  Wijt4  4-  W2^ti  +  wzjU  +  ^4jii  +  >  0}.  (2.30) 

The  set  of  twist  reciprocal  or  repelling  to  all  the  unit  basis  wrenches  in  wj  is 
the  intersection  of  all  the  sets  Ty 

ri»/  =  nw.r2...-}.  (2.31) 

The  set  T,„/  represents  the  set  of  twists  which  are  either  reciprocal  or 
repelling  to  every  unidirectional  unit  basis  wrench  of  a  single  contact.  The 
set  Tinf  describes  only  infinitesimal  motions  which  the  object  is  allowed  to 
undergo  while  constrained  by  a  single  contact.  In  order  to  determine  the 
possible  motions  of  an  object  finite  motions  must  be  considered. 
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Figure  2.6:  The  motioa  of  an  object  is  constrained  by  a  single  contact.  The 
contact  can  be  characterized  by  a  contact  type  which  can  then  be  represented  by 
a  set  of  unidirectional  unit  basis  wrenches  {wj}.  The  set  of  all  twists  which  are 
reciprocal  or  repelling  to  every  basis  wrench,  describes  the  infinitesimal  motions 
the  object  is  allowed  to  undergo. 

2.5  Finite  motions 

The  set  of  twists  Tin/  given  in  equation  2.31  describe  the  infinitesimal  mo¬ 
tion  of  an  object  constrained  by  a  single  contact.  In  order  to  be  useful 
in  determining  all  the  possible  ways  an  actual  object  can  move  within  a 
grasp,  the  definition  of  permissible  motion  must  be  extended  to  include  fi¬ 
nite  motion.  Given  an  arbitrary  unit  twist  t  defined  in  s^^-ew  coordinates 
t  =  [<i,  <2,  fa,  t4,  fs,  <a]  relative  to  some  reference  frame,  let 

■  ti 

h 

t  =  m 

U 
U 
.  ^6 

represent  a  finite  motion,  where  Tnfi,m<2,  and  m<3,  are  finite  rotations  and 
mt4,mti,  and  mi^,  are  finite  translation  along  the  x,  y,  and  z  axes  of  the  ref¬ 
erence  frame.  In  general,  the  twists  in  the  set  Tins  describe  finite  as  well  as 


(2.32) 
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Figure  2.7:  Surface  geometry  in  the  neighborhood  of  the  contact  must  be  con¬ 
sidered  to  determine  the  complete  set  of  small  finite  permissible  motions. 

infinitesimal  motion,  except  for  one  important  exception.  Twists  reciprocal 
to  the  wrench  applied  by  a  point  contact  without  friction  may  or  may  not  be 
permissible.  Whether  motion  is  permitted  depends  on  the  surface  geometry 
in  the  neighborhood  of  the  contact.  Consider  the  following  example  of  two 
dimensional  objects  constrained  by  two  point  contacts  without  friction,  as 
shown  in  figure  2.7.  The  location  and  the  orientation  of  the  contacts  are 
identical,  thus  the  set  of  infinitesimal  permissible  twists  for  each  object  is 
the  same.  The  finite  constraint  imposed  on  the  objects,  however,  is  different. 
The  first  object,  with  concave  surfaces,  is  completely  constrained  in  all  di¬ 
rections.  The  second  object,  the  rectangle,  can  only  move  horizontally,  and 
the  last  object,  the  circle,  can  both  translate  or  rotate  between  the  contacts. 
The  difference  in  constraint  comes  not  from  the  contact  types,  but  from  the 
curvature  of  the  surfaces.  In  order  to  determine  the  complete  set  of  permis¬ 
sible  finite  motions,  the  surface  geometry  in  the  neighborhood  of  the  contact 
must  be  considered. 

Therefore,  we  want  to  determine  the  motions  which  do  not  violate  the 
geometric  constraints  imposed  by  the  surface  of  the  object.  To  solve  this 
problem,  assume  an  object  undergoes  a  hypothetical  finite  motion  describe 
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by  a  twist  mt,  in  equation  2.32,  then  determine  whether  this  particular 
motion  causes  the  contact  point  to  penetrate  the  surface  of  the  object,  thus 
violating  the  geometric  constraints.  Let  the  surface  of  the  object  be  describe 
by  a  function  fsur/ace{x,y)  relative  to  the  contact  frame  Oc* ,  Xc, ,  Kc;  >  • 

First,  determine  the  motion  of  the  contact  point  relative  to  this  contact 
frame  as  the  object  undergoes  the  finite  motion  describe  by  mt.  Second,  map 
the  trajectory  of  the  contact  point  onto  the  surface  of  the  object.  Finally, 
compare  the  trajectory  of  the  contact  point  with  its  projection  onto  the  object 
surface  to  determine  whether  this  particular  motion  violates  the  geometric 
constraints. 

When  comparing  the  motion  of  the  surface  of  the  object  relative  to  the 
contact  points,  it  is  unimportant  whether  we  define  the  motion  as  a  twist 
mt  that  the  object  undergoes  relative  to  fixed  set  of  contact  points  or  as  a 
twist  — mt  that  the  contact  points  undergo  relative  to  a  fixed  object.  For 
convenience,  we  will  assume  the  object  remains  fixed  relative  to  the  OXY Z 
frame  and  the  contact  points  move  with  a  twist  — mt.  The  trajectory  of  the 
contact  point  is  given  by 

xt  =  TtXj  +  d,  (2.33) 

where  Tt  is  a  matrix 


Tt  = 


hhClm 

hhClm  +  hSm 

m 


^2^2Clm 

tzhClm  +  m 


m  +  1 

> 


(2.34) 


where  C\m  =  (1  —  cos(m)),  Cm  =  cos(m),  and  s„,  =  sin(m),  Tt  multiplied  by 
Xi  yields  the  displacement  due  to  a  rotation,  while 


(2.35) 

is  a  displacement  due  to  translation.  Equation  2.33  describes  the  displace¬ 
ment  of  the  contact  point  relative  to  the  OXYZ  frame,  but  we  want  to 
know  the  displacement  of  the  contact  point  relative  to  the  contact  frame 
OciXciYciZc^.  Therefore,  let  Xtraj  the  trajectory  the  contact  point  relative 
to  the  contact  frame.  Then  Xtraj  is  given  by 

Xtraj  =  T^jXt, 


d  = 


mti 

m<5 

mts 


(2.36) 
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where  T,.  is  a  rotation  matrix  relating  points  in  the  contact  frame  0^ , 
to  points  in  the  reference  frame  OXY Z, 


m* 

my 


(2.37) 


Equation  2.36  describes  a  locus  of  points  relative  to  the  contact  frame. 
These  points  represent  the  trajectory  of  the  contact  point  would  undergo  if 
the  object  were  displaced  by  a  twist  mt.  For  a  given  twist,  the  trajectory  of 
the  contact  point  is  a  function  of  only  one  variable,  the  magnitude  m 


^traj 

’  fxim)  ' 

^traj  ~ 

ytraj 

fyi^) 

.  fzim)  . 

By  projecting  the  trajectory  of  contact  onto  the  surface  of  the  object,  it  is 
possible  to  determine  whether  a  particular  twist  t  describes  a  permissible 
finite  motion.  Figure  2.8  shows  the  motion  of  the  contact  point  and  its 
projection  onto  the  surface.  The  surface  of  the  object  is  described  by  the 
equation 

^Burfaee  ~  /surfacei^tV)'  (2.39) 

and  the  projection  of  the  contact  trajectory  onto  the  surface  of  the  object, 
is  given  by 


Xproj 

’  fx{m) 

^proj  ~ 

Vproj 

fvi^) 

.  /»ttr/oce[/*(m), /y(m)].  _ 

(2.40) 


For  a  motion  to  be  permissible,  the  contact  trajectory  must  not  penetrate 
the  surface.  Simply 

^traj  ^  ^proj-  (2*41) 

Therefore,  the  set  of  finite  motions  the  object  may  undergo  is 

Tfin  =  {t :  (2-42) 


Finally,  the  set  of  all  finite  motions  of  an  object  subject  to  a  single  constraint 
is  the  intersection  of  the  finite  and  infinitesimal  permissible  motions 

T  =  T/in  n  ^«n/- 


(2.43) 
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z 


contact  trajectory 
surface  trajectory 


Figure  2.8:  The  projection  of  the  contact  trajectory  onto  the  object  surface 
reveals  whether  a  particular  motion  is  possible.  A  motion  which  causes  the  contact 
to  penetrate  the  sur&ce  is  not  allowed,  while  motions  which  cause  the  object  to 
move  away  from  the  surface  are  permitted. 
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2.5.1  Value  of  virtual  coefficient 

The  definitions  in  the  previous  section  provide  mathematical  constraints 
which  work  well  for  abstract  models.  An  infinitesimal  variation  in  the  di¬ 
rection  of  the  unit  twist,  however,  will  change  its  definition  from  reciprocal 
to  repelling  or  contrary.  In  reality,  small  changes  in  constraint  should  not 
lead  to  drastic  changes  on  the  object.  This  problem  occurs  because  a  twist  is 
defined  to  be  reciprocal  only  when  the  virtual  coefficient  is  exactly  zero.  In 
order  to  compensate  for  inaccuracies  in  contact  measurement  and  to  allow 
more  generality  in  object  motion,  it  would  be  helpful  to  define  a  range  of 
values  ,  —u>r  to  for  which  a  twist  is  defined  to  be  reciprocal.  That  is,  a 
reciprocal  unit  twist  satisfies 


—  UJt<  +  “'4^1  +  ^"5*2  +  ^^6*1  <  (2-44) 

This,  however,  also  changes  the  definition  of  the  repelling  and  contrary  twist. 
A  repelling  unit  twist  will  now  be  defined  as 


+  tv^ts  -b  wzte  +  +  wstz  +  weti  >  (2.45) 

and  a  contrary  unit  twist 


Wit^  -f  wjtz  +  Wzte  +  <  — Wr-  (2.46) 

For  example,  consider  the  case  of  two  fingers  grasping  a  rectangle  in  two 
dimensions,  as  shown  in  figure  2.9.  If  tactile  sensors  are  used  to  resolve  the 
contact  point  and  surface  normal,  there  is  to  be  expected  some  small  error 
in  the  reading,  figure  2.9.  It  is  obvious  that  the  block  can  slide  between  the 
fingertips  in  either  direction  if  the  grasp  is  loosened  sufficiently.  The  strict 
definition  of  the  reciprocal  twist,  however,  indicates  only  one  or  the  other 
direction  is  possible,  depending  upon  error  in  the  sensed  surface  normal.  By 
expanding  the  definition  of  the  reciprocal  twist,  in  equation  2.5,  we  can  again 
obtain  the  correct  result,  that  the  rectangle  can  move  in  either  horizontal 
direction.  Now  that  the  definition  of  the  reciprocal  twist,  equation  2.23,  hM 
been  expanded,  the  geometric  constraints  will  become  more  important  when 
determining  possible  motion. 
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Figure  2.9;  Small  errors  in  the  definition  of  the  surface  normad  of  the  contact 
on  a  two  dimensional  rectangle  lead  to  large  variation  in  the  predicted  physical 
system.  By  expanding  the  definition  of  the  reciprocal  twist,  these  errors  can  be 
accounted  for  as  well  as  yield  more  intuitive  results  in  determining  the  allowed 
motions  of  an  object  within  a  grasp. 
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2.6  Multiple  contacts 

If  there  are  m  contacts  between  the  manipulator  and  the  object  and  n  dif¬ 
ferent  types  of  contacts,  there  will  be  n”*  possible  combinations  of  contacts. 
Each  combination  of  contacts  yield  a  different  set  of  constraints  on  the  ob¬ 
ject.  An  object  may  be  said  to  be  in  a  particular  constraint  state  defined  by 
the  number  and  the  types  of  contacts.  For  m  contacts  and  n  contact  types 
there  are  n”*  possible  constraint  states.  For  example,  if  a  three  fingered  hand 
grasped  an  object  at  the  fingertips  there  are  three  contacts  and,  since  there 
are  only  fingertip  contacts,  there  are  four  contact  types.  With  three  contacts 
and  only  four  possible  contact  types  there  are  4^  possible  combinations,  that 
is  64  constraint  states.  For  graisps  made  with  a  human  hand  at  the  finger¬ 
tips,  there  are  now  five  contacts,  and  therefore  4®  or  2048  possible  constraint 
states.  A  constraint  state  can  be  denoted  by  an  ordered  list  of  m  elements, 
each  element  denoting  the  contact  type.  Suppose  the  three  fingered  hand 
has  its  fingers  numbered  one,  two,  and  three,  and  assume  the  hand  grasps  an 
object  at  the  fingertips.  If  the  first  and  second  fingers  behave  as  soft  finger 
contacts,  while  the  third  finger  slips  on  the  surface,  the  constraint  state  will 
be  [1,1,3].  Similarly,  if  a  human  hand  graisped  a  cup,  for  example,  and  the 
thumb,  index  finger,  middle  finger,  make  soft  finger  contacts  on  the  object, 
the  ring  finger  slips  on  the  surface,  and  the  small  finger  is  removed,  the 
constraint  state  would  be  represented  by  [1,1, 1,3, 4).  More  generally,  the 
constraint  state  is  denoted  by 


C  —  [Ci,  C2,  . .  •  ,  Cni], 

where  c^  is  the  contact  type  for  the  contact. 


(2.47) 


2.7  Permissible  Motion 

From  the  previous  analysis,  the  set  of  permissible  motions  for  au  object 
constrained  by  a  single  contact  was  given  by  T,  equation  2.43.  Now  assume 
the  object  is  constrained  by  multiple  contacts  and  let  the  set  of  permitted 
motions  for  each  contact  be  given  by  where  c,-  represents  the  contact 
type  for  the  contact. 

If  there  are  m  constraints  on  the  object,  the  set  of  permissible  twists  for 
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a  grasped  object  is  the  intersection  of  all  the 

7[cicj...cm]  “  ri{^CciJ>  •  •  •  »  (2-48) 

The  goal  of  this  chapter  was  to  describe  all  the  possible  ways  an  object  may 
move  within  a  particular  grasp.  Equation  2.48  describes,  in  terms  of  twists, 
the  set  of  all  permissible  finite  motions  that  an  object  may  undergo  in  a  given 
grasp. 


Chapter  3 


Strategies  for  manipulation 


3.1  Introduction 

In  this  chapter,  we  wiU  examine  different  external  forces  that  can  act  on 
an  object  and  how  they  can  be  used  to  move  the  object  around  in  a  graisp. 
An  object  may  be  influenced  by  a  number  of  other  forces  besides  the  grasp¬ 
ing  fingers  of  a  robot.  Gravity,  acceleration,  electro-magnetic  forces,  other 
objects,  and  free  fingers  can  all  exert  forces  on  the  object.  Although  these 
forces  differ,  they  can  all  be  collected  into  a  single  external  wrench  acting  on 
the  object.  The  object  will  then  tend  to  undergo  a  twist  repelling  to  this 
external  wrench.  The  set  of  twists  repelling  to  the  external  wrench  can  be 
called  the  preferred  twists,  since  they  describe  the  possible  motions  the  object 
can  undergo.  The  actual  motion,  however,  will  depend  on  the  constraints  on 
the  object.  That  is,  in  order  for  object  to  actually  move,  the  set  of  preferred 
twists  must  intersect  the  set  of  permissible  twists,  discussed  in  chapter  2. 

We  will  begin  by  examining  a  general  strategy  for  using  the  external 
wrench  to  move  an  object  in  a  grasp.  Then  we  will  examine  different  external 
wrenches  including  gravity,  controlled  accelerations,  and  forces  produced  by 
the  robot  and  the  environment.  For  each  of  these  external  wrenches,  different 
controlled  slip  manipulation  strategies  will  be  investigated. 
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3.2  General  strategy 

Assume  a  grasped  object  is  subject  to  an  external  wrench  We.  The  object 
will  then  tend  to  undergo  a  twist  repelling  to  this  external  wrench.  The  set 
of  repelling  twist,  or  preferred  twists,  is  given  by 

Tp  =  +  We,t5  +  W^^te  +  +  Wgjs  >  0}.  (3.1) 

In  order  for  the  object  to  actually  move,  however,  the  set  of  preferred  twist 
must  intersect  the  set  of  permissible  twists.  That  is, 

. Cml>  (3*2) 

where  is  the  set  of  all  possible  motions  the  object  may  undergo  while 

in  the  [ci, . . . ,  Cm]  constraint  state. 

The  set  of  twists  described  in  equation  3.2  is  of  particular  interest,  since 
this  set  will  describe  the  way  the  object  will  actually  move.  If  T  =  0,  then 
the  object  is  completely  constrained  and  will  not  move  at  all.  If  T  contains 
a  single  twist  t,  then  the  object  will  move  in  a  direction  described  by  this 
twist.  This  case  is  especially  important  in  controlled  slip  manipulation,  since 
the  geometry  constraints  and  the  external  wrench  together  specify  a  unique 
motion  of  an  object  within  the  grasp.  However,  if  T  contains  more  than  a 
single  element,  the  resulting  motion  cannot  be  determined  from  the  geometric 
analysis  alone.  Dynamics,  external  forces,  internal  grasping  forces,  and  local 
surface  friction  properties  must  all  be  taken  into  account  to  determine  the 
exact  object  motion.  Although  this  type  motion  is  difficult  to  control  even 
for  humans,  it  should  not  be  ignored  in  robotic  manipulation,  since  it  may 
be  useful  in  predicting  motion  resulting  from  inadvertent  slipping  or  more 
complex  dexterous  motions.  In  fact,  geometrically  unconstrained  motion  was 
analyzed  by  [Mason],  in  which  he  predicted  the  motion  of  an  object  sliding 
in  a  plane  while  subject  to  a  specific  velocity  at  a  single  point. 

The  objective  of  controlled  slip  manipulation  is  to  control  the  motion  of 
an  object  relative  to  the  gr^p.  There  are  some  motions  which  are  easy  to 
accomplish  through  controlled  slip  manipulation.  These  are  motions  which 
are  both  allowed  by  the  constraints  on  the  object  and  bound  by  the  geometry 
contacts.  In  other  words,  if  the  intersection  of  the  permissible  and  preferred 
twists,  equation  3.2,  is  a  small  bounded  set  of  the  twists,  then  these  motions 
which  can  be  easily  implemented  by  a  robot.  Therefore,  when  planning 
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controlled  slip  motions,  consideration  should  be  given  to  these  constrained 
twists  as  well  as  the  twists  which  are  particularly  desirable.  In  any  case,  a 
desired  twist  is  selected  t^,  describing  the  motion  the  object  should  undergo 
in  the  grasp  of  the  robot.  In  order  to  facilitate  this  desired  motion,  the 
external  wrench  on  the  object  should  be  optimized  so  that  it  will  move  the 
object  through  the  desired  twist.  The  optimal  external  wrench  is  one  in  the 
virtual  work  of  the  wrench  against  the  desired  twist  is  a  maximum.  These 
external  wrenches  are 

{we|max{a;(we,td)}}.  (3.3) 

In  the  following  sections,  we  will  examine  different  ways  to  exert  an  ex¬ 
ternal  wrench  on  a  grasped  object  and  how  these  external  wrenches  may  be 
used  to  control  the  slipping  motion  of  an  object  within  a  grasp. 


3.3  Specific  strategies 

3.3.1  Gravity 

People  use  gravity  to  their  advantage  when  manipulating  objects.  We  repo¬ 
sition  objects  in  our  hands  by  allowing  then  to  drop,  slide,  or  rotate  between 
our  fingers.  Consider,  for  example,  when  a  glass  of  water  is  raised  from  the 
table.  We  can  allow  the  gl2iss  to  rotate  between  our  fingertips  so  that  the 
glass  remains  vertical.  Relative  to  our  hand,  gravity  has  been  used  to  rotate 
the  glass  and  in  this  way  maintain  the  vertical  orientation.  Gravity  can  also 
be  used  by  a  robot  manipulator  to  reposition  objects  within  a  grasp.  The 
force  imposed  by  gravity  can  be  modeled  as  a  single  force  at  the  centroid  of 
the  object.  Hand  orientation  can  then  be  used  to  allow  gravity  to  move  the 
object  through  a  desired  twist. 

Suppose  we  wish  to  use  gravity  to  move  an  object  within  a  grasp.  First, 
we  specify  a  desired  twist,  tj  ,  that  is,  a  twist,  relative  to  the  gr2isp,  through 
which  we  want  the  object  to  move.  Then,  by  reorienting  the  object,  the 
external  wrench  We  relative  to  the  grasp,  can  be  varied.  The  virtual  work 
created  by  the  external  wrench  against  the  desired  twist  is  a  function  of  th^f 
object  orientation.  It  is  then  possible  to  determine  a  set  of  object  orientations 
for  which  the  virtual  work  is  a  maximum  and  at  these  orientations  gravity 
will  be  optimally  used  to  move  the  object. 
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To  illustrate  how  gravity  might  be  used,  suppose  a  desired  twist  is 
specified  in  screw  coordinates  relative  to  an  object  frame  OoXoYgZi,.  The 
object  frame  is  a  coordinate  system  whose  origin  is  the  centroid  of  the  object 
and  whose  axes  are  fixed  within  the  object.  Now  let  the  object  frame  be 
defined  in  terms  of  the  hand  frame  frame  OkXhYhZ^,  where  the  z  axis  of 
the  hand  frame  is  parallel  to  gravity,  figure  3.1  and  the  x  and  y  axes  are 
defined  for  a  specific  hand.  The  hand  coordinate  system  for  the  Salisbury 
robot  hand  is  described  in  appendix  A.  In  this  example,  suppose  the  axes  of 
the  object  frame  are  initially  aligned  with  the  axes  of  the  hand  frame.  The 
wrench  caused  by  gravity  acting  on  the  object  defined  relative  to  the  object 
frame  is 


■  Wi  ■ 

■  0 

W2 

0 

W3 

-mg 

W4 

0 

Ws 

0 

.  ^6  . 

.  0 

(3.4) 


where  g  is  the  gravitational  acceleration  and  the  m  is  the  mass  of  the  object. 
However,  if  the  object  frame  is  rotated,  the  value  of  the  external  wrench 
defined  with  respect  to  the  object  frame  changes.  The  Salisbury  robot  hand 
has  the  ability  to  rotate  the  object  through  arbitrary  angles  about  the  axes  of 
the  hand  frame.  Therefore,  assume  the  object  frame  is  rotated  by  an  angle  9 
about  the  x  axis  and  then  by  an  angle  (f>  about  the  y  axis  of  the  hand  frame. 
Now  the  external  wrench  in  terms  of  the  object  frame  will  be 


■  Wi  ' 

— mg  COS  9  sin 

XV2 

—mg  sin  9 

W3 

—mg  cos  5  cos  ^ 

W4 

0 

Ws 

0 

.  ^6  . 

0 

Only  two  rotations  are  used  in  equation  3.5,  since  rotations  about  the  z-axis 
of  the  reference  frame  produce  no  change  in  the  wrench. 

For  certain  values  of  9  and  <p  the  value  of  the  virtual  work  is  a  maximum. 
At  these  orientations,  the  axes  of  the  gravitational  wrench  and  the  desired 
twist  are  the  same.  Given  the  virtual  work 


tv  =  —Tng{cosdsin((>tc^  +sin^<cs  +  cos  5cos 


(3.6) 
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Figure  3.1:  The  wrist  can  be  used  to  orient  the  object  in  a  grasp  so  that 
gravity  can  be  used  to  move  the  object 

the  values  of  0  and  <l>  which  produce  a  critical  values  of  virtual  work  are 


Substituting  the  values  for  9  and  <l>  into  equation  3.6  yield  either  positive  or 
negative  values  for  the  virtual  work.  Values  of  9  and  <t>  for  which  the  virtual 
work  is  positive,  produce  the  maximum  value  of  virtual  work. 

Therefore,  by  using  the  wrist  to  rotate  the  hand  about  the  angles  given  in 
equation  3.7,  the  robot  can  use  the  gravitational  force  to  maximum  efficiency 
when  moving  an  object  within  the  grasp.  The  use  of  gravity  can  be  an 
effective  means  for  controlling  the  motion  of  an  object  within  the  grasp. 
However,  it  may  also  be  somewhat  difficult,  since  the  external  force  cannot 
be  controlled;  all  the  control  must  come  from  the  wrist  and  the  fingertips. 
The  manipulator  must  also  respond  quickly  to  the  motion  of  the  object, 
since  once  it  begins  to  slide  it  will  continue  to  slide.  Other  methods,  such 
as,  controlled  accelerations  and  point  forces  from  both  non- gras  ping  fingers 
and  external  objects,  can  be  used  as  a  more  controlled  means  of  producing 


CHAPTER  3.  STRATEGIES  FOR  MANIPULATION 


32 


an  external  wrench  on  the  object. 

3.3.2  Controlled  accelerations 

It  is  possible  to  accelerate  hand  to  create  a  force  on  an  object.  For  example, 
we  loosen  our  grasp  and  flip  an  object  in  our  hands,  we  have  used  an  ac¬ 
celeration  to  create  a  force  on  the  object  to  produce  a  desired  motion.  The 
strategy  is  to  create  an  external  wrench  on  the  object  while  adjusting  the 
grasp  so  that  the  object  is  in  a  constraint  state  which  will  allow  it  to  slip. 

Using  the  definitions  in  the  previous  section,  we  will  find  the  direction 
the  hand  should  be  accelerated  to  move  the  object  through  a  desired  twist. 
Again,  assume  a  desired  motion  is  specified  by  a  twist  defined  in  screw 
coordinates  relative  to  the  object  frame.  To  maximize  the  virtual  work,  the 
direciton  of  the  acceleration,  in  terms  of  the  object  frame,  should  be  in  the 
same  direction  as  the  twist.  Simply, 

a*  tdj 

a  =s  ^  ^<<3 

<**  I  t<i  1 

.  **»  .  .  . 

Although,  the  use  of  controlled  accelerations  is  one  way  to  move  an  object 
within  the  grasp,  by  far  the  most  common  and  effective  way  to  reorient  an 
object  within  a  grasp  is  the  use  of  free  finger,  that  is,  fingers  not  directly 
involved  with  the  grasp,  and  other  objects. 

3.3.3  Free  fingers 

Fingers  not  involved  with  the  actual  grasp  can  be  used  to  move  the  object. 
This  is  very  common  in  human  manipulation.  We  can  reorient  a  pen  ,  for 
example,  by  holding  it  between  two  of  our  fingers  and  spinning  it  with  a  third. 
A  multifingered  robot  can  also  use  free  fingers  to  manipulate  objects.  While 
some  fingers  constrain  the  object,  the  others  have  the  freedom  to  reorient 
it  within  the  grasp.  This  may  also  be  an  arg\iment  for  mtiltifingered  hands, 
since  the  additional  fingers  allow  greater  flexibility  in  producing  constraints 


(3.8) 
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on  an  object  as  well  as  allowing  the  free  fingers  to  manipulate  object  within 
the  grasp. 

Suppose  the  surface  of  the  object  can  be  represented  by  a  set  of  points  S 
and  for  each  of  the  i  free  fingers  there  is  a  subset  of  S  of  accessible  surface 
points,  Si  C  S. 

Figure  3.2  shows  an  object  located  with  respect  to  a  reference  frame 
OXY Z.  For  each  point  x  €  ■S'i,  a  specific  set  of  forces  can  be  exerted  through 
the  contact.  Assume  the  forces  lie  within  the  firiction  cone  at  the  contact. 
That  is,  the  set  of  wrenches  which  can  be  exerted  in  terms  of  the  contact 
frame  OaXaVciZci,  are 

^  (3.9) 

where  /i  is  the  coefficient  of  friction.  The  set  of  wrenches  defined  relative  to 
the  reference  frame  is 

{we  =  TiWj}.  (3.10) 

The  wrenches  in  equation  3.7  are  a  combination  of  both  the  accessible  surface 
points  which  the  free  finger  can  reach  and  the  forces  which  it  can  exert 
through  each  contact  point.  Suppose  a  twist  is  defined  defined  relative  to 
the  reference  frame  OXY Z.  The  values  for  the  virtual  work  of  the  twist  tj 
against  the  wrenches  in  3.7  is 

W  =  {wlw  =  (3.11) 

The  combination  of  wrenches  and  accessible  surface  points  which  maximizes 
the  virtual  work  can  be  expressed  as  a  set  of  pairs 

{(w,x)|(w,x)  €  max{W}}.  (3.12) 

3.3.4  Other  objects 

Other  objects  can  also  be  used  effectively  to  control  the  motion  of  an  object 
within  a  grasp.  In  the  jar  example,  given  in  the  introduction,  we  use  the 
edge  of  the  jar  to  rotate  the  lid  between  our  fingertips.  A  robot  could  also 
exploit  objects  in  the  environment  to  reposition  an  object  within  a  grasp. 
The  strategy  and  the  analysis  for  this  type  of  manipulation  are  identical  to 
the  free  finger  analysis.  Only  the  accessible  surface  points  Si  are  different. 


CHAPTER  3.  STRATEGIES  FOR  MANIPULATION 


34 


I 

i 


Figure  3.2:  A  force  exerted  on  a  grasped  object  can  move  it  through  a  desired 
motion 

since  the  points  on  the  object  open  to  the  environment  may  be  larger  than 
those  accessible  to  the  fingers.  The  external  wrench  is  one  variable  the  robot 
can  control;  the  next  chapter  will  describe  another  controllable  variable,  the 
grasping  force. 


Chapter  4 
Grasping  force 


4.1  Introduction 

There  are  many  ways  for  a  multifingered  hand  to  squeeze  an  object.  People 
grasp  objects  to  secure  them  in  their  hand  or,  by  carefully  controlling  the 
internal  forces,  allow  objects  to  slide  through  their  fingers.  A  robot  can  also 
use  the  internal  grasping  force  to  create  both  stable  and  unstable  grasps.  By 
regulating  the  grasping  force,  the  robot  can  either  hold  the  object  securely 
or  by  control  the  constraint  state  to  allow  desired  slipping  motion. 

The  variety  of  ways  a  hand  can  squeeze  an  object  depends  on  the  number 
and  type  of  contacts  which  exist  between  the  hand  and  the  object.  The  goal 
of  this  chapter  then  is  to  determine  the  space  of  possible  grasping  forces  on 
an  object  and  to  develop  a  simple  intuitive  parameterization  of  the  squeezing 
force  space  for  two  and  three  fingered  grasps. 


4.2  Grasp  force  analysis 

Assume  the  manipulator  exerts  a  wrench  on  the  object 


^gl  = 


w 

w. 

w, 

w, 

w, 

w 
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defined  in  screw  coordinates  relative  to  the  contact  frame  Og^Xc^Yc^Zci■  Sup¬ 
pose  that  the  contact  frame  can  be  defined  in  terms  of  a  common  reference 
frame  OXY Z,  as  shown  in  figure  4.1.  The  wrench  in  terms  of  the  reference 
frame  is 

W|  =  TiWg.,  (4.2) 

where  Tj  is  the  transformation  matrix  discussed  in  chapter  2.  The  sum  of 
the  wrenches  from  n  contacts  between  the  robot  and  the  object  is 

w  =  f;wi.  (4.3) 

Assume  the  object  is  not  subject  to  any  external  wrench  and  is  not  acceler¬ 
ating.  The  sum  of  the  wrenches  in  equation  4.3  is  then  zero, 

w  =  0.  (4.4) 

One  additional  constraint  will  be  assumed.  It  is  aissume  that  only  forces 
through  a  point  contact  contribute  to  the  internal  grasping  force.  The  mo¬ 
ments  at  the  contact  point  will  be  assumed  to  be  zero,  so  that  the  wrench, 
in  terms  of  the  contact  frame,  is  given  by  Wgj  =  10^3,0, 0,0].  With 

these  assumptions,  it  is  now  possible  to  determine  the  wrench  at  each  con¬ 
tact  point  relative  to  the  contact  frame,  as  a  function  of  the  internal  grasping 
force. 

4.3  Two  contacts 

Assume  the  robot  touches  an  object  at  only  two  points.  Therefore  the  sum 
of  these  wrenches,  equation  4.4,  is 

TiWg^  +  T2Wg2  =  0.  (4.5) 

There  are  six  unknowns  in  equation  4.5.  These  are  the  wrenches,  Wj  = 
[tWjij ,  Wg2i ,  Wg3^ ,  0, 0, 0]  and  W2  =  Wg2,,  Wg^ ,  0, 0, 0).  However,  equation 

4.5  yields  only  five  linear  independent  equations.  Therefore,  there  is  a  one 
space  of  solutions  for  which  equation  4.5  is  true. 

The  one  space  of  solutions  can  be  thought  of  as  an  arbitrary  squeezing 
force,  given  simply  by  a  grasp  force  magnitude  rrig.  Once  the  grasp  force 
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2c. 


Figure  4.1:  Assume  each  of  the  grasping  fingers  can  exert  a  wrench  Wc.  defined 
in  ternas  of  the  contact  frame  through  the  contact  point.  The  wrench  defined 
with  respect  to  the  common  reference  frame  is  Wj  =  TiWcj,  where  Tj  is  the 
transformation  matrix  discussed  in  chapter  2.  The  sum  of  each  of  the  wrenches 
is  w  =  53^1  Wj  and  if  the  object  is  neither  subject  to  external  wrenches  nor 
accelerating,  this  wrench  w  is  zero. 
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magnitude  is  specified,  the  values  of  the  wrenches  Wg^  and  Wgg  can  be 
found. 

Let  the  grasp  force  magnitude  be  given  by 


m,  =  5;  y'tnfi,  + 


(4.6) 


1=1 


The  screw  coordinates  of  the  wrench  due  to  internal  grasping  force  can  be 
found  more  easily  by  considering  the  wrenches  in  terms  of  an  axis,  a  pitch,  and 
a  magnitude.  Assume  the  two  contact  forces  are  described  by  wrenches  Wi 
and  W2,  defined  by  wrenches  axes  Ai  and  A2,  zero  pitches,  and  magnitudes 
mi  and  respectively.  Since  the  object  is  neither  subject  to  an  external 
wrench  nor  accelerating,  both  the  wrench  axes  must  intersect  the  contact 
points  and  have  opposite  directions,  figure  4.3.  The  wrench  axes  are  given 
by, 

(ii  -  Xj) 

(yi  -  ya) 

{z\  -  Zi) 

-^i(yi  -  ya)  +  yi(^i  -  -^a) 

Zl{Xi  -  Xj)  -Xi{zi-  Z3) 

-yi(xi  -xj)  +  xi(yi  -ya) 
where  J  is  the  distance  between  the  contact  points 


±  Ai  =  TA2  = 


(4.7) 


d=  \/(xi  - 1,)’  +  (»i  -  yj)’  +  (z,  -  r,)’. 


(4.8) 


Notice  the  direction  of  the  axes  in  equation  4.7  are  opposite,  yet  the  value 
of  one  of  them  may  be  either  positive  or  negative.  This  analysis  does  not 
determine  the  sign  in  equation  4.7,  since  this  will  depend  on  the  physical 
system.  For  example,  if  two  fingers  grasp  a  block,  the  wrench  axes  would 
be  directed  into  the  block,  but  if  the  fingers  grasp  inside  a  ring,  the  wrench 
axes  would  be  directed  outward.  Therefore  the  direction  of  the  wrench  axes 
will  depend  on  the  direction  of  the  contact  normal.  The  direction  of  the  axis 
which  yields  a  negative  value  of  the  dot  product  between  the  contact  normal 
and  the  wrench  axis  will  be  the  correct  direction. 

The  sum  of  the  individual  magnitudes  is  given  in  equation  4.6 


Tlig  —  TTli  +  m2. 


(4.9) 
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The  magnitudes  of  the  wrenches,  however,  must  be  equal,  since  if  one  ex¬ 
ceeds  the  other,  there  will  be  a  net  wrench  on  the  body  contradicting  the 
equilibrium  assumption;  thus. 


mi  =  mj. 


(4.10) 


Then  by  equation  4.9  and  4.10 


mi  =  mj  =  mj/2. 


(4.11) 


The  wrenches  Wi  and  W2  are  now  uniquely  defined.  The  wrench  axes  are 
given  in  equation  4.7,  the  magnitudes  in  equation  4.11,  and  the  pitch  of  both 
wrenches  is  zero. 

It  is  useful,  in  the  later  analysis,  to  express  the  wrenches  in  terms  of  a 
reference  frame  and  the  contact  frame.  The  wrenches  Wj  defined  in  terms  of 
screw  coordinates  relative  to  the  reference  frame  are 


Wj  =  — W2 


(xi  -  X2) 

{yi  -  ya) 

(zi  -  Z2) 

-ziivi  -y7)  +  yiizi  -Z2) 

Zi(Xi  -  X2)  -  Xi{Zi  -  Z2) 
-yi{xi  ~  X2)  +  Xi(yi  -  yj)  . 


(4.12) 


The  screw  coordinates  of  the  wrenches  in  terms  of  the  contact  frame  is  a 
linear  transformation  of  the  elements  in  equation  4.12, 


Wgj  =  Tj^wi 

(4.13) 

^g2  =  T^^W2, 

(4.14) 

where  Tj"^  is  the  inverse  of  the  linear  transformation  matrix  discussed  in 
chapter  2. 


4.4  Three  contacts 

For  a  three  fingered  hand,  such  as  the  Salisbury  or  Okada  [Okada]  robots, 
three  fingertip  contacts  are  possible.  Assume  the  robot  touches  the  ob¬ 
ject  at  three  points  and  can  only  exert  forces  through  the  contacts,  then 
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Figure  4.2:  If  only  two  fingertip  touch  an  object  and  we  assume  only  forces 
are  exerted  through  the  contact,  the  direction  of  the  force  vectors  lie  on  the  line 
intersecting  the  contact  points.  If  the  contact  forces  are  described  in  terms  of 
wrenches,  the  axes  of  wrenches  intersect  the  two  contact  points,  the  magnitudes 
are  identical,  and  the  pitches  are  both  zero. 
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the  wrench  at  each  contact  defined  in  terms  of  the  contact  frame  is  w„.  = 
[wgi^ ,  Wg2i ,  1^33, ,  0, 0, 0]  and  the  wrench,  in  screw  coordinates,  defined  in  terms 
of  a  reference  frame  is 

Wi  =  TiWg..  ■  (4.15) 

The  sum  of  the  wrenches,  from  equation  4.4,  is 

TiWgj  +  T2Wg2  +  TaWgg  =  0.  (4.16) 

There  are  nine  unknowns  in  equation  4.16,  wi  =  0)0,0], 

W2  =  [iW3i,,tU32,,iU33,,0,0,0],  and  W2  =  (1^312,  u^32j,ii>a3j)  0,0,0],  and  equa¬ 
tion  4.16  yields  only  six  independent  equations.  This  still  leaves  three  inde¬ 
terminant  variables.  The  three  space  of  possible  solutions  can  be  represented 
fairly  simply.  The  wrench  axes  of  each  of  the  three  wrenches  will,  in  general, 
intersect  at  a  point,  the  grasp  force  focus.  The  grasp  force  focus  must  lie  on 
the  grasp  plane,  defined  to  be  the  plane  containing  the  three  contact  points. 
In  general,  the  grasp  plane  will  be  well  defined;  however,  if  the  contact  points 
lie  along  a  single  line,  the  plane  collapses  to  a  line  and  the  grasp  focus  lies  on 
this  line.  If  the  grasp  plane  exists,  the  grasp  force  focus,  may  lie  anywhere 
on  the  plane.  As  long  as  there  is  no  external  wrench  on  the  object  and  the 
object  is  not  accelerating,  the  wrenches  will  intersect  at  a  point,  with  one 
exception:  the  wrench  axes  may  all  be  parallel  and  still  be  in  equilibrium.  In 
this  case,  the  grasp  force  focxis  parameterization  can  still  be  used  if  we  also 
consider  points  at  infinity  on  the  grasp  plane. 

There  is  a  third  variable,  the  grasp  force  magnitude  nig.  The  grzisp  force 
magnitude  is  a  scaling  factor  by  which  individual  wrench  values  are  multi¬ 
plied.  Together  the  grasp  force  focus  and  the  grasp  force  magnitude  span 
the  entire  three  space  of  internal  graisp  solutions.  By  choosing  a  graisp  force 
focus  Xg  pg  on  the  grasp  plane  and  with  a  grasp  force  magnitude  rUg,  we  can 
uniquely  define  the  internal  squeezing  force  created  by  a  three  fingered  grasp. 

To  find  the  wrenches  resulting  from  this  parameterization  of  the  grasp 
force,  we  first  find  the  wrenches  in  terms  of  the  grasp  frame.  The  grasp 
frame  is  the  coordinate  system  whose  x  and  y  axes  lie  on  the  grasp  plane  and 
origin  is  fixed  at  the  centroid  of  the  three  contact  points.  Let  wj  represent  the 
wrench  defined  relative  to  the  grasp  frame  Xj  =  0]  represent  a  contact 

point.  The  wrench  axes  Aj  of  each  of  the  wrenches  will  be  the  direction 
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vectors  from  the  contact  points  xj  to  the  grasp  force  focus  Xg 


A 


/ 

i 


Xg-x'i 

Va  -  Vi 
0 
0 
0 


(4.17) 


L  -vK^g  -  x'i)  +  x'iivg  -  Vi)  J 

where  |<f,(  =  \/(Xg  —  +  (t/g  —  y,-)*.  The  sign  used  in  equation  4.17  de¬ 

pends  on  geometry.  If  the  fingers  were  grasping  a  sphere  and  the  grasp  force 
focus  was  in  the  center  of  the  sphere,  the  sign  of  A-  would  be  positive.  Con¬ 
versely,  if  the  fingers  were  grasping  a  ring  from  the  inside  the  sign  of  Aj 
would  be  negative.  In  general,  the  sign  depends  on  the  direction  of  the  nor¬ 
mal  to  the  contact.  The  sign  should  be  chosen  so  that  the  wrench  axis  and 
the  contact  normal  are  in  the  same  direction.  Let  the  grasp  force  magnitude 
be  the  sum  of  the  individual  wrench  magnitudes,  m,-, 


mj  =  mi  4-  7712  -I-  m3.  (4.18) 

Since  the  wrench  axes  are  given  by  Aj,  in  equation  4.17,  and  the  magnitudes 
are  given  by  m,-,  the  first  two  elements  of  the  wrench  in  screw  coordinates 
relative  to  the  grasp  frame  are 

w[.  =  mi{xg  -  x\)l\di\  (4.19) 

^2,  =  ^iiVa  -  y[)l\di\-  (4.20) 

(4.21) 

By  equation  4.4,  the  sum  of  the  elements  in  equation  4.21  must  sum  to  zero. 
Together  with  equation  4.18  this  yields  three  linearly  independent  equations 
and  three  unknowns, 


■  (x,  -  xi)/ldi|  (x,  -  xi)/|d2l  (x,  -  x'3)/|d3|  ■ 

’  mi  ■ 

0 

{ya-y'xWi\  {ya-y'2)l\d2\  {yg-y'^VW 

m2 

= 

0 

1  1  1  . 

.  ^3  . 

.  ^a  . 

(4.22) 
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Solve  for  the  magnitudes  m,-, 


mi 

r(x,-xi)/idii 

(Xg  -  Xi)/ld2l 

(Xg  -  x'3)/ld3l  ■ 

-1 

0  ■ 

m2 

= 

(yg-PiWil 

(yj;  -  y3)/l<^3l 

0 

m3 

1 

1 

1 

.  ^g  . 

(4.23) 

The  matrix  in  equations  4.22  and  4.23  will  be  invertible  except  when  the 
three  contact  points  lie  on  a  line  and  the  grasp  force  focus  does  not.  In  this 
case,  choose  a  grasp  force  focus  on  the  line  and  then  the  magnitude  will  be 
proportional  to  the  distance  from  the  contact  points  to  the  focus, 


m<  =  mjld,|.  (4.24) 

In  any  case  equation  4.17  defines  the  wrench  axes,  equations  4.23  and  4.24 
defines  the  wrench  magnitudes,  and  pitch  are  all  zero.  The  wrench  resulting 
from  the  internal  force  at  each  contact  is  now  defined  by  a  grasp  force  focus 
Xg  and  yg  and  a  grasp  force  magnitude  nig. 

The  wrenches  in  screw  coordinates  relative  to  the  grasp  frame  are 


Wi 


X,  - 1; 

Vs  -  Vi 

Zg  — 

-<iyg  -  Vi)  +  Vii^g  -  ^i)  ’ 

-  x<)  -  x;(x,  -  xO 
-y'iixg  -  xj)  +  x'iivg  -  y'i)  . 


(4.25) 


and  in  terms  of  the  contact  frames 

^g|  =  Tiwj.  (4.26) 

where  T|  is  the  linear  transformation  relating  the  wrench  in  the  contact  frame 
to  the  grasp  frame. 


4.5  Four  or  more  fingered  grasps 

For  four  fingered  hand  like  the  MIT/Utah  hand,  [Jacobsen],  there  are  twelve 
unknowns  and  only  six  equations,  yielding  a  six  space  of  internal  grasping 
force  solutions.  For  the  human  hand,  for  manipulators  with  six  or  more 
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Figure  4.3:  There  is  a  three  space  of  solutions  for  the  internal  grasping  force  for 
a  three  fingered  hand.  The  axes  of  the  force  vectors  from  the  individual  contacts 
must  intersect  at  a  point,  the  grasp  force  focus  Xg  =  [a:a,yj,0]  in  the  grasp  plane. 
The  grasp  force  magnitude  mg  scales  the  individual  fingertip  forces.  Together,  the 
grasp  force  centroid  and  the  grasp  force  magnitude  span  the  three  space  of  internal 
grasp  solutions. 

fingers,  and  for  grasps  involve  more  than  five  contacts,  the  dimension  of  the 
internal  force  space  grows  very  large.  The  dimension  of  the  solution  space 
for  internal  grasp  force  is 

Dim{G}  =  Z(N  -2)  +  U,  (4.27) 


where  G  is  the  space  of  internal  grasp  solutions,  N  is  the  number  of  contacts, 
and  U  is  the  number  of  freedoms  of  the  grasped  object. 


Chapter  5 

Contact  wrenches 


5.1  Introduction 

In  order  to  determine  the  different  ways  an  object  can  move  in  a  grasp,  the 
constraint  state  of  the  object  must  be  known.  The  constraint  state  of  the  ob¬ 
ject  can  be  found  knowing  the  location,  orientation,  and  the  types  of  contacts 
that  exist  between  the  object  and  the  manipulator.  A  tactile  sensor  might 
supply  contact  location  and  orientation  information,  but  a  sensor  which  de¬ 
termines  the  contact  type  does  not  exist.  In  order  to  find  the  contact  type  at 
the  fingertips  of  the  robot,  the  contact  wrench,  that  is  the  set  of  forces  and 
moments  which  exist  at  the  contact  point  relative  to  the  contact  hame,  must 
be  found,  as  well  as,  a  relationship  between  the  contact  wrench  to  the  con¬ 
tact  type.  The  purpose  of  this  chapter  is  to  determine  the  contact  wrench. 
The  purpose  of  the  next  chapter  will  be  to  determine  a  simple  relationship 
between  the  contact  wrench  and  the  contact  type.  In  this  way  we  can  find 
the  constraint  state  of  the  object  as  a  function  of  the  grasping  force  and  the 
external  forces. 


5.2  Stiffness 

Assume  a  hemispherical  compliant  fingertip  touches  a  flat  plate,  as  shown  in 
figure  5.2.  To  begin  the  analysis,  assume  the  interface  between  the  fingertip 
and  the  object  can  be  modeled  as  a  soft  finger  contact.  That  is,  both  forces 
normal  and  tangent  to  the  surface  as  well  as  moments  about  the  surface  nor- 
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mal  can  be  transmitted  through  the  contact  point.  A  system  of  translational 
and  rotational  springs  attached  to  the  contact  point  will  be  used  to  model 
the  fingertip,  as  shown  in  figure  5.2.  The  stiffnesses  of  the  fingertip  can 
be  represented  as  a  matrix  relating  the  twist  to  the  wrench 

kci  0  0 

0  0  kc2  0 

0  0  kc3 

kc4  0  0 

0  kcs  0  0 

a  0  kc^ 

For  a  soft  finger  contact,  the  elements  kc4  and  kc$  are  zero,  that  is,  the 
fingertips  cannot  generate  moments  about  any  axis  which  lies  in  the  tangent 
plane.  However,  for  generality  kc4  and  kc^  are  included,  since  linear  and 
planar  contacts  have  these  stiffness  and  these  contact  types  may  be  used  in 
future  analyses. 

The  stiffness  given  in  equation  5.1  is  defined  in  terms  of  the  contact  frame 
OciXciYc^Zci-  The  fingertip  stiffness  defined  relative  to  a  common  reference 
frame  OXYZ  is 

ki  =  Tikc.Tf^  (5.2) 

where  Tj  is  a  transformation  matrix  of  the  contact  discussed  chapter  2 
equation  2.14.  The  stiffness  of  the  entire  object  will  be  the  sum  of  all  the 
individual  stiffness  matrices 

K  =  f:  ki.  (5.3) 

Assume  the  object  is  displaced  by  a  twist  t,  the  wrench  on  the  object  is  given 

by 

w  =  Kt.  (5.4) 

If  the  object  is  not  accelerating,  the  wrench  given  in  equation  5.4  must  equal 
the  external  wrench  on  the  object,  We, 

w  =  We.  (5.5) 

The  twist  the  object  undergoes  can  be  determined  as  a  function  of  the  ex¬ 
ternal  wrench  on  the  object,  by  taking  the  inverse  of  K  in  equation  5.4, 

t  =  K-*We.  (5.6) 
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Figure  5.1;  The  contact  between  the  manipulator  and  an  object  is  generalized 
as  a  hemispherical  compliant  solid  touching  a  nondeformable  flat  plate. 

The  twist  in  equation  5.6  is  defined  in  screw  coordinates  relative  to  the 
OXY Z  reference  frame.  The  twist  defined  relative  to  the  contact  frame  can 
be  found  by  multiplying  by  the  inverse  of  the  transform  matrix  Ti, 

ti  =  TiK-'we,  (5.7) 

and  the  wrench  at  the  contact,  in  terms  of  the  contact  frame,  is 

W|  =  kc.TiK~^We.  (5.8) 

Add  to  this  the  wrench  due  to  the  grasping  force 

Wi  =  kcjTiK~^We  +  Wgj.  (5.9) 

Thus,  equation  5.9  describes  the  wrench  at  the  contact  point  defined  relative 
to  the  contact  frame  as  a  function  of  the  stiffness  of  the  fingertips,  the  forces 
on  the  body,  and  the  force  the  grasp.  Using  this  result  and  the  results  in  the 
next  chapter,  it  will  be  possible  to  determine  the  contact  type  as  a  function 
of  the  forces  on  the  object  and  the  squeezing  force.  Once  the  contact  types 
are  found,  the  constraint  state  of  the  object  will  be  known. 
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Figure  5.2:  The  fingertip  is  modeled  as  a  set  of  translationad  and  rotational 
springs  connected  to  the  contact  point.  The  stiffnesses  are  defined  in  terms  of  the 
contact  frame.  Since  a  soft  finger  contact  is  assumed,  there  is  a  stifihess  in  the 
normal  and  tangent  directions  of  the  contact  plane,  as  well  as,  a  torsional  stiffness 
about  the  contact  normal. 


Chapter  6 

Contact  wrench/contact  type 
relation 

6.1  Introduction 

The  purpose  of  this  chapter  is  to  determine  a  relationship  between  the  contact 
type  and  the  contact  wrench.  In  general,  this  is  a  difficult  problem.  In  fact, 
determining  slipping  and  twisting  of  one  compliant  body  in  contact  with 
another  is  a  current  research  topic  in  finite  element  analysis.  In  this  chapter, 
therefore,  simplifying  assumptions  will  be  made  to  find  a  simple  relation 
between  contact  type  and  contact  wrench,  so  that  the  overall  problem  of 
determining  the  constraint  state  can  be  tractable. 

6.2  Two  dimensions 

For  the  two  dimensional  problem,  the  relationship  between  the  contact  wrench 
and  the  contact  type  can  be  simple.  Assuming  the  fingertips  erert  only  forces 
though  the  contact  points,  there  are  only  three  contact  types;  a  point  con¬ 
tact  with  friction,  a  point  contact  without  friction,  and  no  contact.  Assuming 
coulomb  friction,  if  the  tangent  force  is  less  than  the  coefficient  of  static  fric¬ 
tion  times  the  normal  force,  the  contact  will  behave  like  a  point  contact 
with  friction.  If  the  tangent  force  exceeds  the  coefficient  of  friction  times  the 
normal  force,  the  contact  will  slide,  b  modeled  as  a  point  contact  without 
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friction.  Given  a  wrench  at  the  contact  point  defined  by 


Wi 

W2 


0 

0 


(6.1) 


where  Wi  force  in  the  tangent  direction,  loj  is  the  force  normal  to  the  surface. 
The  possible  contact  types  are 

1  =  Point  contact  with  friction 

2  =  Point  contact  without  friction  (6.2) 

3  =  No  contact, 

and  the  relationship  between  the  contact  wrench  and  the  contact  types  will 
be 

|uJl|  <  \fiW2\  =»  1 

kil  >  2 

W2  >  0  =►  3. 

For  the  three  dimensional  case,  an  exact  relation  between  the  contact  type 
and  the  contact  wrench  is  difficult;  however,  a  simple  relation  can  be  useful. 


6.3  Three  dimensions 

Determining  when  and  where  a  three  dimensional  compliant  contact  breaks 
or  twists  in  response  to  an  applied  wrench  is  currently  at  the  forefront  of 
finite  element  analysis.  Exact  solutions  are,  in  general,  unknown.  The  force 
distribution  over  the  contact  region  between  two  hemi-ellipsoidal  objects  un¬ 
der  an  axially  applied  load  can,  be  found  by  use  of  a  linearization  argument, 
commonly  known  as  the  Hertz  contact  problem  [Lipson].  This  simple  case 
does  not  take  into  account  loads  applied  tangent  to  the  contact  surface  or  to 
moments  about  the  contact  normal;  however,  we  can  use  the  Hertz  solution 
to  generate  a  simple  relation  between  contact  type  and  contact  wrench. 

Hertz  assumed  two  bodies  in  contact  could  be  modeled  as  solid  elliptic 
disks,  each  possessing  a  minimum  and  maximum  radii  of  R  and  R\  Further¬ 
more,  he  cissumed  that  these  disks  were  in  contact  along  their  common  axes 
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Figure  6.1:  Two  general  solid  elliptic  disks  in  contact  under  an  axially  applied 
load.  The  load  is  assume  small  and  the  disks  somewhat  rigid,  so  that  a  linearization 
argument  may  be  applied  to  determine  the  pressure  distribution  and  the  contact 
region. 


under  a  uniform  axially  applied  load  F,  as  shown  in  figure  6.3.  Hertz  deduced 
that  the  pressure  distribution  between  the  two  bodies  can  be  described  by 
a  semi-ellipsoid  of  pressure  constructed  over  the  surface  of  the  contact,  eis 
shown  in  figure  6.3. 

The  pressure  distribution  is  given  by 


y)  =  Poy/l  -  (6.3) 


The  total  load,  therefore,  is  equal  to  the  volume  of  the  semi-ellipsoid. 


Solve  for  Po 


2‘KabPo 

~ 


(6.4) 


(6.5) 
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P(x,y) 


Figure  6.2:  Hertz  modeled  the  pressure  distribution  in  the  contact  area  between 


the  two  solid  elliptic  disks  as  a  semi-ellipsoid  with  a  minimum  and 
dimensions  a  and  6. 

maximum 

where  a  and  b  are  given  by  Timosheko  [Lipson] 

_ ,/  3FA 

\4iA  +  By 

(6.6) 

.  _  J  SFA 

”V4(A  +  B)’ 

(6.7) 

and 

A  =  (1  -  ^l)/E,  +  (1  -  i.l)/E„ 

(6.8) 

Hi,H2  =  Poisson’s  ratio 


El,  E2  =  Moduli!  of  elasticity 
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^  I/CA  «,)  +2  (a  ZZTZ  cos(2^) 

.  t/?  =  The  angle  between  the  planes  contacting  the  curvatures  l/Ri  and  I/R2 

Ri,  R[,R2,  Rli  =  Mimirnum  and  maximum  radii  of  curvature  of  the  ellisoid  disks  at 
the  point  of  contact 

m,n  =  Constants  depending  on  B  ~  Aj B  A  A 


In  the  case  of  the  contact  between  a  hemispherical  fingertip  and  a  flat  rigid 
plate,  equations  6.6  and  6.7  is 

a  =  6  =  ^3Fi2iA/4,  (6.9) 

and  the  maximum  pressure  is  given  by 

ft  =  (6.10) 

If  the  flat  plate  is  assumed  to  be  rigid  (i.e.  E2  »  Ei)  then  A  can  be 
approximated  by 

A  =  (l-//?)/^i.  (6.11) 

The  material  on  the  fingertip  of  the  Salisbury  robot  hand  is  a  polyure- 
athane  with  an  elastic  modulus  of  approximately  40,000  psi.  and  a  poisson’s 
ratio  of  about  0.45.  The  radius  of  the  fingertip  is  approximately  0.5  in, 
therefore 


A  =  2.0  X  10-* 

Po  =  1247.9  ^Ib/in^ 
a  =  0.020^in. 
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The  pressure  distribution  given  in  equation  6.3  can  now  be  given  in  polar 
coordinates,  since  the  pressure  region  in  this  case  is  given  by  the  area  of  a 

circle.  That  is,  _ 

\/a^  — 

P(r,&)=P,^ - ,  (6.12) 

a 

where  r  is  the  radial  distance  from  the  center  of  the  contact  region.  Again 
assuming  coulomb  friction,  the  maximum  moment  the  contact  can  support 
before  rotating  is  the  integral  over  the  contact  area  of  the  differential  force 
times  the  coefficient  of  static  friction  times  the  radial  distance  from  the  con¬ 
tact  point, 

M  =  n  f  rdf.  (6.13) 

J  A 

Substitute  these  values  into  equation  6.13 


M^.rr 

Jr=0  JS=Q 

and  evaluating  yields 


r=a  ^«=2ir  ZEVo'^  -  ^ 


A/  = 


27ra^ 

iirnaF 
'  16  ■ 


r^drdB, 


Substituting  the  value  for  a 


(6.14) 

(6.15) 


SiTfiF  JSFRjl  -  lil) 

16  V  AEi 


(6.16) 


Thus,  M  is  proportional  to  the  four-thirds  power  of  the  normal  force,  while 
the  constant  of  proportionality  is  a  function  of  constant  properties  of  the 
fingertip  material  and  the  surface, 


M  = 


(6.17) 


where 


Mm  * 


3i2(l-/2?) 


4Ei 


(6.18) 


If  the  specific  values  for  the  Salisbury  robot  fingertips  are  substituted  along 
with  a  value  of  /i  =  1.0  for  the  coefficient  of  static  friction,  the  maximum 
twisting  moment  will  be 


1,15  X  lO"*/'*'’. 


(6.19) 
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Now  ignore  the  moment  and  assume  a  force  is  exerted  tangent  to  the  con¬ 
tact  surface.  This  is  simply  the  case  of  coulomb  friction,  and  the  maximum 
tangent  force  will  be 

Ft  =  /iF.  (6.20) 

In  order  to  further  simplify  this  analysis,  assume  the  maximum  moment 
which  can  be  exerted  about  the  contact  normal  is  a  linear  function  of  the 
normal  force, 

M  =  fimF,  (6.21) 

where  fim  is  given  in  equation  6.18. 

,  The  purpose  of  this  analysis  is  to  determine  the  contact  type  as  a  function 
of  the  contact  wrench.  Given  the  contact  wrench  in  the  screw  coordinates 


w 


■  U>1 
W2 

W3 

W4 

Ws 


L  J 


(6.22) 


where  W4  and  ws  will  automatically  be  zero,  since  we  are  only  considering 
fingertip  contacts.  The  relationship  between  the  contact  wrench  and  the 
contact  type,  given  the  linearizing  aissumptions  discussed  above,  will  be 


where 


y/wl  +  wj 
\Jw\  A  wl 
\Jw\  A  wl 

1U3  >  0 


1 

2 

=}►  3 
=►  4, 


1  Soft  finger  contact 


2  Point  contact  with  friction 

3  Point  contact  without  friction 


(6.23) 


4  No  contact. 
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M 


fxF  Ft 


Figure  6.3:  When  the  force  tangent  to  the  contact  exceeds  fi/  times  the  normal 
force,  assume  the  contact  is  sliding  on  the  surface.  If  the  tangent  force  is  less  than 
M/F,  and  the  moment  about  the  contact  normal  exceeds  fim  times  the  normad 
force,  assume  the  fingertip  is  twisting  relative  to  the  contact  surface.  Finally, 
if  both  the  tangent  force  and  moment  are  less  than  fi/F  and  /XmF  respectively, 
assume  the  contact  is  rigidly  fixed  to  the  surface. 

We  win  approximate  =  1.83  x  10“^  in  and  /i  =  1.0  for  the  Salisbury  robot 
hand  grasping  an  aluminum  can.  With  the  relation  given  in  equation  6.23 
and  the  analysis  of  the  previous  chapter,  it  will  now  be  possible  to  determine 
the  constraint  state  of  a  grasped  object  as  a  function  of  the  external  forces 
on  the  object  and  the  squeezing  force  of  the  hand. 


Chapter  7 

Controlled  Slipping 


7.1  Introduction 

So  far  only  individual  modeling  issues  have  been  discussed,  but  we  have 
not  yet  addressed  how  these  results  can  be  combined  and  how  they  can 
be  used  to  enhance  the  dexterity  of  a  robot  hand.  In  this  chapter,  the 
analyses  of  the  preceding  chapters  will  be  integrated  together  and  the  result 
will  be  used  to  predict  and  affect  some  controlled  slip  motions.  To  illustrate 
these  principles,  two  examples  will  be  considered.  The  first  is  a  simple  two 
dimensional  example  of  two  fingers  holding  a  rectangle  in  a  gravitational  field. 
This  example  outlines  the  analyses  of  the  previous  chapters  in  some  detail 
and  allows  many  of  the  concepts  to  be  graphically  illustrated.  The  second 
example  is  of  a  three  fingered  robot  hand  grasping  a  cylinder.  This  example 
demonstrates  how  controlled  slip  manipulation  might  be  implemented  on  an 
actual  robot  system.  The  next  chapter  describes  how  these  ideas  were  applied 
to  the  multifingered  Salisbury  robot  hand. 


7.2  Two  dimensional  example 

Two  fingers  grzap  a  rectangle  on  opposing  surfaces,  as  shown  in  figure  7.1. 
In  this  particular  example,  the  fingers  grasp  the  block  at  the  center  of  its  top 
and  bottom  sides.  The  block  has  a  height  of  4  cm  and  a  length  of  7  cm  and  is 
held  horizontally  in  a  gravitational  field.  There  are  a  number  of  coordinate 
systems  used  in  this  example,  figure  7.2.  These  are:  the  reference  frame. 


57 


CHAPTER  7.  CONTROLLED  SLIPPING 


58 


hand  frame,  object  frame,  and  contact  frames.  These  coordinate  frames  are 
used  throughout  the  analysis  and  are  described  in  appendix  A. 


Figure  7.1:  Two  fingers  grasp  a  two  dimensional  rectangular  block.  In  this 
example,  we  are  assuming  the  block  only  moves  in  the  xy  plane  and  that  gravity 
exerts  a  force  on  the  block  in  the  negative  y  direction. 


7.2.1  Constraint 

Contact  types 

There  are  three  different  contact  types  for  fingertip  contacts  in  the  plane:  a 
point  contact  with  friction,  a  point  contact  without  friction,  and  no  contact. 
Each  contact  type  allows  only  a  certain  set  of  wrenches  to  be  transmitted 
through  the  interface.  As  with  the  three  dimensional  case,  these  wrenches 
may  be  described  by  the  set  of  unidirectional  unit  basis  wrenches,  which 
span  the  space  of  permissible  wrenches.  For  convenience  each  contact  types 
is  represented  by  a  number  as  follows: 

1  Point  contact  with  friction 

2  Point  contact  without  friction 

3  No  contact 
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'Y., 

Figure  7.2:  There  are  five  coordinate  frames  shown  in  this  example,  the  reference 
frame,  the  hand  frame,  the  object  frame,  and  the  contact  frames.  The  block  has 
dimensions  4x7  cm  and  a  mass  of  0.2  kg. 

Constraint  states 

In  the  example  illustrated  in  figure  7.1,  there  are  only  two  contacts.  With 
three  possible  contact  types  at  each  contact,  yielding  a  total  of  3^  or  nine 
constraint  states.  As  with  the  three  dimensional  case,  the  constraint  state  is 
described  as  an  ordered  list  with  as  many  elements  eis  there  are  contacts.  Let 
the  contact  made  by  the  finger  on  the  top  of  the  rectangle  be  first  element 
in  the  list  and  the  finger  on  the  bottom  be  second.  Thus,  if  the  fingertip 
on  the  top  of  the  block  were  beginning  to  slip,  but  the  finger  on  the  bottom 
remained  fixed,  the  constraint  state  of  the  griped  rectangle  would  be  [2, 1]. 

Permissible  motions 

For  each  constraint  state,  there  are  some  motions  which  the  object  may 
undergo  and  some  which  it  may  not.  As  before,  the  set  of  allowed  motions 
can  be  represented  by  a  set  of  twists.  For  consistency,  the  twist  will  still  be 
represented  by  the  six  element  vector  t  =  (fi,  fa,  fs,  <5,  fg];  however,  for  the 
two  dimensional  case,  the  elements  <1,  fa?  and  fg  are  zero,  the  elements  U 
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and  ts  are  the  translations  in  the  x  and  y  directions,  and  is  the  rotation 
about  the  normal  to  the  plane.  All  the  elements  of  the  twist  are  defined  with 
respect  to  some  reference  frame  OXYZ,  which  in  this  case  coincides  with 
the  object  frame,  figure  7.1. 

In  the  two  dimensional  case,  the  set  of  permissible  twists  may  be  rep¬ 
resented  graphically.  The  direction  of  motion  can  be  described  by  a  vector 
in  three  space,  coordinate  system.  Figure  7.3  shows  a  coordinate  system 
with  axes  labeled  ^4,  fs,  and  ta,  corresponding  to  the  translations  in  the  x 
and  y  directions  and  the  rotation  respectively.  The  direction  of  motion  may 
be  represented  by  a  unit  vector  radiating  from  the  origin,  thus  set  of  twists 
correspond  to  a  collection  of  unit  vectors,  figure  7.4.  If  the  twists  all  lie  in  a 
plane  the  set  is  represented  as  a  section  of  a  shaded  disk,  otherwise  sets  of 
twists  are  shown  as  sections  of  a  sphere,  figures  7.5  and  7.6. 


h 


Figure  7.3:  The  direction  of  motion  in  may  be  represented  using  a  simple  graphic 
technique.  Unit  twists  corresponding  to  the  direction  of  motion  are  illustrated  as 
a  unit  vector  radiating  from  the  origin  of  a  coordinate  system  with  axes  labeled 
t4  and  ts  (translations  in  the  x  and  y  directions),  and  ts  (the  rotations  about  the 
normal  to  the  plane). 

In  the  two  dimensional  Ccise  as  in  the  three,  the  set  of  permissible  motions 
is  the  intersection  of  all  the  unit  twists  repelling  to  the  unit  basis  wrenches 
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Figure  7.4;  Set  of  unit  twist  are  coUections  of  unit  vectors  radiating  from  the 
origin. 


Figure  7.5:  Unit  twists  which  lie  in  a  plane  are  shown  as  section  of  a  shaded  disk 
for  clarity,  otherwise  they  are  shown  as  section  of  a  sphere. 
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^5 


Figure  7.6;  Unit  twists  correspond  to  subsets  of  the  sphere. 

and  unit  twists  reciprocal  to  the  normal  basis  wrenches  which  do  not  violate 
the  geometric  constraints  of  the  contact  surface.  For  this  example,  the  set 
of  unit  twists  repelling  or  reciprocal  to  all  the  unit  basis  wrenches  is  shown 
in  figure  7.7.  Figure  7.8  shows  the  additional  constraint  imposed  by  the 
geometry  of  the  contact  surface.  In  the  diagrams,  permissible  motion  is 
represented  by  either  single  vectors,  shaded  disks  and  sections  of  spheres. 

7.2.2  External  wrench 

A  wrench  in  two  dimensions  may  be  represented  by  the  six  element  vector 
w  =  [wi,W2,  w:i,  U74,  1U5,  Wfi],  where  and  are  zero.  The  elements  w\ 

and  W2  represent  force  in  the  x  and  y  directions  respectively  and  represents 
the  moment  about  the  normal  to  the  plane. 

Gravity 

The  gravitational  force  on  the  object  can  be  used  to  move  the  object  within 
the  grasp.  Assume  the  rectangle  is  in  a  gravitational  field,  with  an  acceler¬ 
ation  g  in  the  negative  y  direction.  Let  the  rectangle  have  a  mass  m.  Then 
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Figure  7.7:  Constraint  states  for  the  rectangular  block  ignoring  the  local 
surface  geometry 
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Figure  7.8:  Constraint  states  for  the  rectangular  block  including  local  surface 
geometry 
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wrench  on  the  object  in  screw  coordinates  defined  with  respect  to  the  OXY Z 
reference  frame  is 

r  0  1 


w  = 


-mg 


0 

0 

0 

0 


(7.1) 


and  with  respect  to  the  object  frame,  it  is 


w 


—mg  sin  6 
—mg  cos  9 
0 
0 
0 
0 


(7.2) 


Suppose  we  can  reorient  the  block  in  the  xy  plane  to  any  angle  9,  figure 
7.9.  Through  the  use  of  gravity,  we  have  means  of  changing  the  wrench  on 
the  rectangle  with  respect  to  the  object  frame.  If  the  block  have  a  mass  of 
0.2039  kg,  the  external  wrench  (in  newtons)  is 


—2%m9 
—2  cos  9 
0 
0 
0 
0 


(7.3) 


7.2.3  Grasping  force 

Since  there  are  two  point  contacts  between  the  hand  and  the  object,  the 
fingers  can  exert  an  arbitrary  squeezing  force.  Defined  relative  to  the  contact 
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Figure  7.9;  The  rectangle  can  be  reoriented  in  a  gravitational  field  to  any  orien¬ 
tation  0 

frames  at  each  of  the  fingers,  this  internal  force  will  be 


Wi  =  W2  = 


0 

-F/2 

0 

0 

0 

0 


(7.4) 


7.2.4  Contact  wrenches 

Since  we  are  only  considering  point  contacts,  in  this  planar  example,  only 
translational  stiffness  is  possible,  as  shown  in  figure  7.2.4.  The  stiffness  nor¬ 
mal  and  tangent  to  the  contact  surface  are  given  by 


(7.5) 


^normal  —  5N  /  Cm 

^tangent  ~  2N/cm. 

The  wrench  at  the  contact  defined  with  respect  to  the  contact  frame  is 
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Figxire  7.10:  Fingertip  contacts  between  a  manipulator  and  an  object  in  two 
dimensions  can  be  replaced  by  a  set  of  two  springs:  a  spring  normal  to  the  contact 
with  a  stiffness  of  knormai  and  a  spring  tangent  to  the  surface  k tangent- 
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wi  =  kcjTi"^K"^We  +  wgi, 


(7.6) 


where, 


1.  fee-  is  6  X  6  element  stiffness  matrix  representing  the  the  stiffness  at  the 
contact  point  in  terms  of  the  contact  frame, 


kcj  - 


0 

0 

0 

-oo 

0 

0 


0 

0 

0 

0 


0 

0 

0 

0 


-k 


— oo  0 

0  0 


tangent 

0 

0 

0 

0 

0 


0 

'^normal 

0 

0 

0 

0 


0 

0 

— oo 
0 
0 
0 


Note  the  values  of  the  stiffness  are  negative,  since  we  are  interested  in 
the  force  on  the  object  as  a  result  of  the  displacement  of  the  fingertips. 
Replaced  kcj  and  kcj  with  the  numerical  values 


kcj  —  kc2  — 


0 

0 

0 

-oo 

0 

0 


0 
0 
0 
0 

— oo  0 

0  0 


0  -2 

0  0 


0 

0 


0 

0 

0 

0 


0 

-5 

0 

0 

0 

0 


0 

0 

— oo 
0 
0 
0 


2.  Tj  is  a  transformation  matrix  for  the  contact  defined  in  chapter  5. 
For  two  dimensions,  this  is 


Ti  = 


m. 


0 


ly  niy  0 
0  0  1 


■  0 

0 

y 

■  /* 

rux 

0  ■ 

0 

0 

—X 

ly 

my 

0 

lyX  IxP 

niyX  —  m*y 

0 

.  0 

0 

1 
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and  the  inverse 


■ 

■  /x 

0  ■ 

m. 

TTly 

0 

0 

0 

0 

1 

■  0 

0 

lyX 

-izy 

■  /x 

0  ' 

0 

0 

TTlyOC 

-rn^y 

771 3- 

TTly 

0 

. 

.  y 

—  X 

0 

0 

0 

1  . 

mi  =  [mj;,Tny]i  and  Hi  =  [n^.,  rij,],-  are  the  tangent  and  normal  vectors; 
and,  Xi  and  j/,  is  the  location  of  the  contact.  Therefore,  in  this 
example. 


and 


and 


T2  = 


Ti  = 


■  -1 
0 
0 

'  0 
0 

.  -1 


[ 

0  ■ 

0 

0 

1  ■ 

0 

0 

0 

I 

■ 

-1 

0 

0  . 

- 

0  0  ■ 

— 

1  0 

0 

0 

1 . 

0 

-1 ' 

r- 

1  0 

01 

0 

0 

0-10 

0 

0 

0  0 

Ij 

• 

I 

0 

'0  0  - 

-1 

0  0 

0 

I 

1  0 

0 

5 


and 


* 

o 

o 

1 

0-10 

0 

o 

o 

1 

o 

o 

o 

o 

1 

0  0  0 

0-10 

1 

o 

o 

.  0  0  1. 
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3.  K  is  the  total  stiffness  of  the  grasped  object 

K=x;ki, 


<=i 


where  kj  is  the  stiffness  resulting  from  the  contact  at  the  origin  of 
the  object  frame, 

ki  =  Tik.jTr\ 

where  Ti  and  are  given  above  and 


ki  = 


and 


kj  = 


0 

0 

— oo 
— 2oo 
0 
0 

0 

0 

—  00 
— 2oo 
0 
0 


0 
0 
0 
0 

— oo 
0  -2 


2  -2 

0  0 

0  0 

0  0 

0  0 


0 

-5 

0 

0 

0 

0 


0 

0 

0 

0 

-oo 

0 


2 

0 

0 

0 

0 

-2 


-2 

0 

0 

0 

0 

2 


0 

0 

0 

0 


0 

0 

— oo 
— oo 
0 
0 

0 

0 

— oo 
oo 
0 
0 


Thus 


K  = 


and 


'-1 


0 

0 

0 

-4 

0 

0 

0 

0 

0 

0 

-10 

0 

0 

0 

0 

0 

0 

2o 

4oo 

0 

0 

0 

0 
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0 
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0 

0 

0 

0 

0 

-4 

0 

0 

0 

■  0 

0 

0  0 

0 

0  ■ 

0 

0 

0  0 

0 

0 

0 

0 

0  0 

0 

1 
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0 
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0 

0 

1 
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4.  We  is  the  external  wrench, 


We 


— 2sin^ 
—2  cos  6 
0 
0 
0 
0 


5.  Wg.  is  the  internal  wrench. 


0 

-F/2 

0 

0 

0 

0 


Substituting  the  values  for  this  particular  example,  the  contact  wrenches  are 


and 


Wi 


—  sin^ 

—  cos  9  —  F/2 
0 
0 
0 
0 


W2 


sin^ 

cos  9  —  F/2 
0 
0 
0 
0 


(7.7) 


(7.8) 


Plots  of  the  contact  wrenches  as  a  function  of  the  squeeze  force,  F,  and  the 
orientation  9  are  given  in  figure  7.11. 
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Finger  1 

Tangent  force  formal  force 


Finger  2 
Tangent  force 


Normal  force 
W2 


Figure  7.11:  The  normal  force  and  the  tangent  force  at  each  contact,  relative  to 
the  contact  frame,  as  a  function  of  the  squeezing  force  F  and  the  orientation  of 
the  block  ff. 


CHAPTER  7.  CONTROLLED  SLIPPING 


73 


7.2.5  Contact  wrench  /  Contact  type  relation 

The  relation  between  the  contact  wrench  and  the  contact  type  for  two  di¬ 
mensions  is  given  in  chapter  6, 


[u;i|  <  [/iiual  1  (point  contact  with  friction) 

1^1 1  ^  2  (point  contact  without  friction) 

W2  >  0  =>  3  (no  contact). 

Instead  of  using  the  absolute  values,  it  the  above  relations,  it  would  be  more 
convenient  to  determine  an  analytic  function.  Let  5,-  be  a  slipping  criteria 
defined  by 


Therefore  the  relation  between  the  contact  wrench  and  the  contact  type  is 
5,  >  1  =>  1  (point  contact  with  friction) 

5,-  <  1  ^  2  (point  contact  without  friction) 

102,  >1  3  (no  contact). 


The  plots  for  Si  and  S^  are  shown  in  figure  7.12,  for  values  of  grasping  force 
ranging  from  2N  to  ION.  Si  will  approach  infinity  in  some  orientations,  as 
the  grasping  force  is  reduced  to  zero.  Also,  as  might  be  expected,  the  slipping 
criteria  will  tend  toward  zero  as  the  grasping  force  is  increased  to  infinity, 
independent  of  the  orientation  of  the  object.  Whenever  the  rectangle  is  held 
in  a  horizontal  position,  the  slipping  criteria  Si  and  S2  are  both  zero,  since 
this  orientation  relies  on  mechanical  rather  than  frictional  constraints.  The 
mapping  of  5,  can  therefore  be  used  as  a  measure  of  stable  grasps.  The 
function  Si  will  be  a  minimum  on  each  contact  when  the  ratio  of  frictional 
force  to  normal  force  is  a  minimum;  that  is.  Si  will  be  a  minimum  when 
the  grasp  depends  most  on  geometric  rather  than  frictional  constraint.  The 
maximum  values  of  Si  are  at  ir/4  and  77r/4  and  the  maximum  values  S2  are 
at  37r/4  and  57r/4.  Intuitively,  this  is  because  the  block’s  weight  is  supported 
more  by  the  finger  on  the  bottom,  unweighting  the  top  finger.  Since  the 
tangent  force  on  both  fingers  is  the  same,  the  ratio  Si,  of  the  top  finger,  is 
a  maximum.  The  rectangle  is  still  constrained,  since  the  geometry  of  the 
surface  does  not  allow  rotation.  If  the  surfaces  were  convex,  however,  the 
object  might  drop  out.  In  the  next  section  regions  of  constraint  will  be 
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defined  as  a  function  of  object  orientation  and  grasping  force. 

7.2.6  Constraint  state  map 

A  useful  plot,  in  terms  of  controlled  slip  analysis,  is  a  constraint  state  map, 
which  defines  the  constraint  state  as  a  function  of  the  controllable  variables, 
such  as  squeeze  force,  orientation,  and  externally  applied  forces.  In  this  par¬ 
ticular  example,  the  constraint  states  are  a  function  of  the  block  orientation, 
6,  and  squeeze  force  m^,  figure  7.13 

Suppose  the  rectangle  is  held  at  a  30“  angle  relative  to  the  horizontal  with 
a  squeeze  force  of  4  N,  represented  as  a  point  on  the  constraint  state  map,  as 
shown  in  the  figure  7.13.  By  varying  the  orientation  and  the  grasping  force, 
it  is  possible  to  move  about  the  constraint  state  map,  arbitrarily  entering 
and  exiting  different  constraint  regions  to  either  avoid  unstable  grasps  or 
to  produce  the  desired  slipping  motions.  For  example,  suppose  we  want  to 
reorient  the  block  by  sliding  it  between  the  fingers.  First,  determine  which 
constraint  states,  if  any,  allowed  this  motion.  As  can  be  seen  in  the  graphs 
in  figure  7.8,  there  are  four  such  constraint  states:  [22],  [23],  [32],  and  [33]. 
Second,  from  among  these  constraint  states  choose  the  one  which  produces 
the  maximum  constraint  on  the  object,  thus  minimizing  inadvertent  motion. 
In  this  case,  constraint  state  [22]  is  the  maximally  constrained  state  which 
allows  the  desired  motion.  Third,  to  produce  this  constraint,  it  is  necessary 
to  move  from  the  present  location  in  region  [l  1]  in  the  constraint  state  map  to 
any  point  in  region  [22],  as  shown  in  figure  7.13.  In  the  region  [22]  the  block 
may  slide  through  the  fingers,  but  the  direction  of  motion  is  determined  by 
the  orientation  of  the  block.  More  generally,  the  direction  of  motion  is  the 
intersection  of  the  permissible  and  preferred  twist,  which  in  this  case  yields 
a  single  vector. 

Consider  another  example.  Suppose  two  fingers  grasp  adjacent  sides  of 
a  square,  as  shown  in  figure  7.14.  Using  the  assumptions  as  in  the  previous 
case,  the  graphs  of  permissible  twists  are  shown  in  figure  7.15  and  a  map  of 
constraint  state  is  produced  in  figure  7.16.  Therefore  to  ensure  a  stable  grasp, 
it  is  necessary  to  remain  in  the  constraint  state  region  [11].  If  the  grasping 
force  is  increased  the  constraint  state  changes  from  [11]  to  [22],  which  re¬ 
sults  in  an  unstable  grasp.  The  subsequent  slipping  motion  in  this  region 
is  bounded  as  shown  in  figure  7.15,  but  its  exact  direction  is  unpredictable. 
Constraint  regions  [12]  and  [21]  yield  sets  of  permissible  twists  which  contain 
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Figure  7.12:  This  is  a  plot  of  the  slipping  criteria  for  the  fingers  on  the  block. 
The  plot  for  finger  contact  on  the  top  of  the  block  is  on  the  top  of  the  page  and 
the  plot  for  the  finger  on  the  bottom  is  on  the  bottom  of  the  page.  When  the 
curve  is  a  minimum,  the  ratio  of  the  tangent  and  normal  force  at  the  contact  is  a 
minimum.  In  other  words,  there  is  a  greater  dependence  on  structural  constraint 
rather  than  frictional  constraint  at  minimums  of  the  Si  plots. 
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only  a  single  element;  however,  these  sets  of  permissible  twists  do  not  inter¬ 
sect  the  set  of  preferred  twists  when  the  object  is  moved  into  these  constraint 
regions. 

Using  the  constraint  state  map,  along  with  maps  of  the  permissible  twists, 
it  is  possible  to  analyze  and  actuate  controlled  slip  motion  for  grasped  ob¬ 
jects.  In  the  next  section,  a  more  general  three  dimensional  example  will  be 
considered. 


7.3  Three  dimensional  example 

The  three  dimensional  analysis  is  essentially  the  same  as  is  two  dimensions. 
The  goal  is  to  generate  a  map  of  the  different  constraint  states  as  a  function 
of  the  controllable  variables,  such  as  the  external  wrench,  grasping  force, 
stiffness,  etc.  Consider  the  Salisbury  three  fingered  robot  hand  grasping  a 
can,  as  shown  in  figure  7.17.  The  robot  has  two  fingers  on  the  top  of  the  can 
and  a  third  on  the  bottom,  equally  spaced  between  the  top  two.  The  grasp 
is  somewhat  off  center  from  the  centroid  of  the  can. 

Figure  7.18  shows  the  coordinate  frames  which  will  be  used  the  analy¬ 
sis:  the  hand  frame  0/iX/iY\Zh,  the  object  frame  OoX^YoZo,  the  grasp  frame 
OgXgYgZg,  aud  three  contact  frames  OiXiYiZi.  Descriptions  of  these  coor¬ 
dinate  frames  are  given  in  appendix  A.  The  centroid  of  the  can  is  given  by 
a  point  in  the  hand  frame  Xcg  =  [—0.8, 2.7,  —3.0]  cm.  The  can  has  a  diam¬ 
eter  of  2.5  in  (6.4  cm)  and  length  of  5.0  in  (12.2  cm),  and  mass  0.386kg.  In 
this  example,  the  hand  holds  the  can  at  a  45®  angle  in  a  gravitational  field. 
Therefore,  the  external  wrench  on  the  can,  in  terms  of  the  hand  frame,  is 

■  0 
0.273 
-0.273 
“  0.082 
-0.218 
.  -0.218 

By  changing  the  orientation  of  the  can  or  by  squeezing  1' ,  we  ca.n  effectively 
control  the  constraint  state.  Suppose  we  only  consider  changes  in  the  grasp¬ 
ing  force  [xg^  yg,  rrig].  The  constraint  state  map  will  then  be  a  three  dimension 
map,  where  the  constraint  states  will  be  a  function  of  Xg,  yg,  and  rug.  Figure 


N. 


(7.10) 
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7.19  shows  two  “slices”  for  constant  grasp  force  magnitude,  nig  =  0.7N  and 
nig  =  2.QN.  The  shaded  areas  represent  regions  in  which  the  can  is  fully 
constrained  and  will  not  move. 

Suppose  the  grasp  force  focus  is  given  by  Xg  =  0.0cm,  yg  =  —1.2cm, 
and  nig  =  0.7N,  corresponding  to  a  point  A  within  the  [2,2,2]  region  of  the 
nig  =  0.7N  slice  of  the  constraint  state  map.  A  reorientation  of  the  can  is 
possible,  in  this  example,  by  allowing  it  to  rotate  between  to  of  the  fingers. 
Regions  [3,2,2]  and  [4,2,2]  allow  a  single  rotation  about  the  second  and  third 
fingers.  Therefore  by  moving  the  grasp  force  focus  from  the  current  location 
in  the  [2,2,2]  region  to  any  point  in  the  [3,2,2]  region,  the  can  will  rotation 
between  the  fingers  as  desired. 

As  in  the  two  dimensional  case,  we  can  choose  an  arbitrary  slipping  mo¬ 
tion  and  determined  the  maximally  constrained  state  which  allowed  the  de¬ 
sired  motion.  This  constraint  state  can  then  be  produced  by  adjusting  the 
controllable  variables  such  as  grasping  force  and  orientation,  as  indicated 
by  the  constraint  state  map.  In  the  next  chapter,  we  will  implement  this 
procedure  on  a  robot  hand. 


CHAPTER  7.  CONTROLLED  SLIPPING 


78 


Figure  7.13:  The  map  of  the  constraint  states  as  a  function  of  the  gra  oing 
force  and  the  object  orientation  can  be  used  both  to  find  stable  grasps  securing 
the  object  and  unstable  grasps  allowing  the  object  to  slip.  In  this  example,  the 
current  state  of  the  grasped  rectangle  is  shown  as  point  A  in  the  figure.  By  moving 
along  the  indicated  path,  the  constraint  of  the  object  can  be  change  from  [11]  to 
[22].  Once  in  constraint  state  [22],  the  rectangle  is  to  slide  between  the  fingers. 
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Figure  7.14:  Two  fingers  grasp  a  square  on  adjacent  sides.  Intuitively  we  know 
that  this  type  of  grasp  is  more  difficult  to  maintain,  eis  shown  in  the  subsequent 
figures. 
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Figure  7,15;  Permissible  motions  for  square  grasped  on  adjoining  sides. 
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Figure  7.16;  This  is  a  map  of  the  constraint  states  els  a  function  of  the  grasping 
force  and  the  orientation  of  a  square  held  by  fingertips  on  adjacent  sides.  Notice 
that  simply  increasing  the  grasp  force  does  not  guarantee  a  stable  grasp.  A  stable 
grasp  is  only  possible  in  the  shaded  [11]  region.  Any  other  region  in  the  constraint 
state  map  will  result  in  an  unpredictable  slipping  motion. 
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Figure  7.17:  By  controlling  the  grasping  force,  the  can  be  held  securely  or 
reoriented  by  controlled  slip. 


Zs 


Figure  7.18:  There  are  six  coordinate  frames  used  in  this  example:  the  hand 
frame,  the  object  frame,  the  grasp  frame,  and  the  contact  frames.  The  hand  is 
rotated  45®  is  a  gravity  field. 
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Figure  7.19:  The  constraint  state  map,  shown  here,  is  a  function  of  the  grasping 
force,  Xg,  yg,  and  nig.  Two  “slices”  of  the  three  dimensional  constraint  state  map 
are  shown,  for  nig  =  0.7iV  and  nig  =  2.0N .  The  current  position  on  the  constraint 
state  map  is  shown  as  a  point  A  in  the  [2,2,2j  region,  within  the  nig  =  0.7iV  slice. 
By  moving  from  the  present  position  in  the  map  to  any  point  in  the  [3,2,2]  region 
the  can  is  free  to  rotate  between  the  fingertips. 


Chapter  8 

Implementation 


8.1  Introduction 

The  purpose  of  controlled  slip  manipulation  is  to  enhance  the  dexterity  of 
a  multifingered  robot  manipulator,  giving  a  robot  hand  a  larger  repertoire 
of  manipulation  strategies.  A  number  of  programs  were  written  to  automat¬ 
ically  analyze  the  constraints  on  an  object  within  a  particular  grasp.  One 
function  accepts  a  desired  motion  and  returns  the  maximally  constrained 
state  which  allows  that  twist.  Another  determines  the  constraint  state  on 
the  object  as  a  function  of  the  grasping  force.  All  these  functions  were  writ¬ 
ten  in  LISP  and  were  used  in  conjunction  with  the  Salisbury  robot  hand. 
The  following  sections  describe  the  hardware  of  the  robot  system  and  the 
software  implementation  of  the  slip  analyses. 


8.2  Description  of  Hardware 

The  Salisbury  robot  hand  is  shown  in  figure  8.2.  The  hand  has  three  fingers 
and  three  joints  in  each  finger,  allowing  a  total  of  nine  degrees  of  mechanical 
freedom-  Each  of  the  fingers  is  controlled  by  four  steel  tendons  running 
to  torque  motors  in  the  forearm.  Linear  encoders  on  the  motors  and  tendon 
tension  sensors  in  the  fingers  determine  the  joint  position  and  torque  allowing 
the  hand  to  operate  in  a  position,  force,  or  stiffness  control  mode. 

The  general  control  structure  is  shown  in  figure  8.2  Two  modified  Puma 
robot  controllers  accomplish  the  lower  level  servo  control,  while  higher  level 
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Figure  8.1:  The  Salisbury  robot  hand  has  three  lingeis  and  three  joints  in  each 
finger,  yielding  a  total  of  nine  degrees  of  mechanical  freedom.  The  hand  has  linear 
encoders  on  the  motors  to  determine  joint  position  and  tension  sensors  on  the 
tendons  to  resdve  the  joint  torque. 

fingertip  and  joint  trajectories  are  directed  jErom  the  VAX  11/750.  At  a  still 
higher  level,  the  S3anbolics  3600  controls  the  coordinated  hand  functions. 


8.3  Description  of  software 

A  program,  GRASP,  was  written  to  analyze  the  constraints  of  an  object 
grasped  by  the  three  fingered  robot  hand.  The  program  was  designed  to 
be  interactive,  allowing  the  user  to  select  specific  grasp  analysis  functions, 
control  the  location  and  orientation  of  the  grasp,  or  to  choose  a  particular 
squeeze  force.  The  functions  can  be  selected  from  a  menu,  and  the  results  of 
the  analyses  are  displayed  on  a  graphics  window,  illiistrated  in  figure  8.3.  The 
screen  is  divided  into  three  regions.  The  large  pane  to  the  left  is  a  graphics 
screen  showing  the  hand,  finger,  object,  and  contact  locations.  The  smaller 
screen  on  the  top  right  is  LISP  Listener,  which  accepts  and  interprets  LISP 
commands.  Finally,  on  the  bottom  right,  a  small  message  monitor  screen 
prints  messages  which  are  sent  and  received  from  the  VAX.  The  floating 


CHAPTER  8.  IMPLEMENTATION 


86 


Figure  8.2:  The  robot  hand  three  basic  levels  of  control.  At  the  lowest  level 
PUMA  robot  controllers  implements  the  high  speed  servo  control,  at  the  inter¬ 
mediate  level  a  VAX  11/750  controls  the  joint  trajectory  and  finger  stiffness,  and 
Finally,  at  the  highest  level  a  Symbolics  3600  LISP  Machine  manages  the  coordi¬ 
nated  hand  motion. 
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menu  allows  the  user  to  select  many  graphic  and  actuation  options.  Each 
of  the  options  will  be  described  briefly  in  the  following  sections.  Functions 
which  employed  the  controlled  slip  analyses  will  be  described  in  more  detail. 

8.3.1  GRASP  functions 

Standard  Graphics  Options: 

CREATE  GRASP  SCREEN.  Creates  the  three  paneled  display  screen 
CLEARSCREEN.  Clears  the  graphics  screen 

DRAW  COORDINATE  SYSTEM.  Draws  the  six  coordinate  systems  on  the 
graphics  screen.  These  are  the  hand  frame,  object  frame,  grasp  frame, 
and  the  three  contact  frames.  These  different  coordinate  systems  are 
fully  described  in  appendix  A. 

CHANGE  GRAPHICS  VARIABLES.  Allows  the  user  to  change  the  scahng 
and  the  graphics  screen  origin. 

Basic  Actuation  functions: 

GO  HOME.  Reinitializes  the  VAX  and  Lisp  Machine  trajectory  lists  and 
goes  to  the  home  position 

REINIT  VAX  TRAJECTORY.  Reinitializes  the  trajectory  list  on  the  VAX 

REINIT  OOLAH  TRAJECTORY.  Reinitializes  the  trajectory  list  on  the 
LISP  machine 

BASIC  MOVES.  Allows  the  users  to  specify  coordinated  finger  motion.  The 
options  allow  the  user  to  translate  or  rotate  the  grasp  frame  in  any 
desired  cartesian  direction. 

MOVE  FINGER.  Moves  a  selected  finger  by  a  specified  displacement 
Controlled  slip  analyses: 

CHANGE  GLOBAL  VARIABLES.  Allows  any  of  the  global  variables  to  be 
changed. 
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Creatf  araao  screen 
Clear,  icrteii  x 
Draw  coordinate  system 
Change  graohics  variables 


SLIP  ANALYSIS  OPTIONS: 
Change  global  variables 
Permissible-twist 
Oetermine  constraint  state 
Hao  constraint  space 


Remit  system 
Reinit  OOLAH  trajectory 
Remit  UAX  trajectory 
Go  home 
BASIC  MOVES 
Move  finger 

ADVANCED  HAND  ACTUATION  OPTIONS- 
JKS  move  menu 
Move  to  contact  points 
Pick  grasp  force  center 
Controlled  slip 
DEMONSTRATIONS 


1. 
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Figure  8.3:  The  GRASP  program  was  written  as  an  interactive  interface  between 
the  user  and  the  Salisbury  robot  hand  system.  The  program  allows  the  user 
to  select  a  number  of  functions,  including:  slip  analysis  functions,  functions  to 
relocate  and  reorient  the  grasp,  and  a  function  which  changes  the  grasp  force  on 
the  object. 
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PERMISSIBLE  TWIST.  Specify  a  desired  twist  and  this  function  wiU  return 
the  maximally  constrained  state  of  the  object  that  allows  that  twist. 
This  is  accomplished  simply  by  looking  at  each  contact  individually 
and  determining  which  contact  type  will  allow  the  specified  twist.  The 
contact  types  are  investigated  in  decreasing  order  of  constraint.  That 
is,  a  soft  finger  contact,  a  point  contact  with  friction,  a  point  contact 
without  friction,  and  no  contact  were  considered  sequentially.  When  a 
particular  contact  type  allows  the  a;  icified  motion,  this  contact  type  is 
stored.  The  contact  types  are  then  compiled  into  a  list,  thus  yielding 
the  constraint  state  which  allows  the  desired  motion.  This  constraint 
state  is  also  maximal,  since  any  state  of  greater  constraint  would  not 
allow  the  twist  to  occur  at  one  or  more  of  the  contacts. 

DETERMINE  CONSTRAINT  STATE.  This  option  allows  the  user  to  spec¬ 
ify  the  grasping  force,  [xg,yg,mg],  and  returns  the  constraint  state  of 
the  object  for  a  specific  orientation  and  a  given  grasp.  The  program 
uses  the  stiffness  model  described  in  chapter  5  together  with  the  contact 
type/ contact  wrench  relation  given  in  chapter  6. 

MAP  CONSTRAINT  SPACE.  The  user  specifies  the  magnitude  of  the  grasp¬ 
ing  force,  TUg,  and  the  program  produces  a  map  of  the  constraint  states, 
by  varying  the  grasp  force  focus,  [xg,yg\.  The  program  plots  the  re¬ 
gions  of  different  constraint  and  prints  the  value  of  the  constraint  state 
in  each  region.  For  example,  figure  8.4  shows  a  slice  of  the  constraint 
state  map  for  m,  =  T.OiV  for  the  same  situation  described  in  chapter 
7.  Notice  that  the  coordinate  systems  of  the  hand,  the  grasp  frame, 
and  contact  frames  are  drawn  in  the  display,  since  the  grasp  force  focus 
corresponds  to  an  actual  point  in  space.  From  this  point  the  user  can 
select  the  grasping  force  [xj,  by  using  the  option  PICK  GRASP 

FORCE  CENTER. 

Advanced  Hand  Actuation  Options: 

MOVE  TO  CONTACT  POINTS.  Moves  the  fingertips  to  specified  contact 
points  on  the  object.  This  function  takes  into  account  the  radius  of  the 
fingertip  when  grasping  an  object 

PICK  GRASP  FORCE  CENTER.  The  user  specifies  a  grasp  force  magni¬ 
tude  rrig.  Then  the  program  draws  the  grasp  frame  coordinate  system 
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on  the  screen,  along  with  the  hand,  and  contact  coordinate  systems. 
The  user  may  then  move  to  any  point  in  the  graphics  screen  and  select 
a  grasp  force  focus,  [xg,yj],  on  the  grasp  plane.  The  robot  will  then 
displace  the  fingers  by  an  amount  and  a  direction  proportional  to  the 
specified  grasp  force,  [xg,yg,mg\.  Given  the  compliance  of  the  fingers, 
the  resulting  force  will  be  proportional  to  the  desired  internal  grasping 
force  at  each  of  the  fingers. 

DEMONSTRATIONS.  Three  demonstration  routines  were  written  to  illus¬ 
trated  how  controlled  slipping  techniques  might  be  used  on  an  actual 
robot  hand.  The  three  routines  show  respectively  how  gravity,  free  fin¬ 
gers,  or  other  objects  may  be  used  to  reorient  and  reposition  an  object 
within  a  grasp. 

The  first  demonstration  is  the  three  dimensional  example  of  chapter 
7.  The  robot  hand  holds  a  full  can  of  liquid,  as  shown  in  figure  8.5. 
The  mass,  location,  orientation,  and  surface  properties  of  the  can  are 
the  same  as  those  given  in  chapter  7.  A  constraint  state  map  shown 
in  figure  8.6,  for  a  constant  grasp  force  magnitude  of  0.7N.  Initially, 
a  grasp  force,  [xg,yg,mg]  =  [0.0cm., —1.2cm.,  0.7iV]  is  selected  so  that 
the  can  is  in  the  [2,2,2],  a  fully  constrained  region  of  the  constraint 
state  map.  The  magnitude  of  the  grasp  force  remains  the  same,  but 
the  focus  is  moved  out  of  the  [2,2,2]  region  into  the  adjacent  [3,2,2] 
region.  The  result,  shown  in  figure  8.7,  is  that  the  can  spins  between 
the  two  fingers,  under  the  influence  of  gravity,  into  a  new  orientation. 
The  grasp  force  focus  is  then  moved  back  into  the  [2,2,2]  region  and 
the  grasp  is  again  secured;  however,  the  can  has  rotated  90®. 

In  the  second  demonstration,  the  robot  hand  again  holds  a  can  in  the 
same  position  and  orientation.  One  of  the  fingers  is  then  removed  from 
the  top  of  the  can.  The  situation  is  essentially  a  two  fingered  grasp  or 
a  three  fingered  grasp  with  the  first  finger  in  a  type  4  contact.  In  any 
case,  the  magnitude  of  the  squeezing  force  between  the  two  grasping 
fingers  can  be  controlled.  In  this  case,  the  magnitude  of  the  squeezing 
force  is  relaxed,  so  that  when  the  free  finger  exerts  a  force  on  the  front 
of  the  can,  the  constraint  moves  into  the  [2,4,2]  constraint  region  and 
spins  between  the  two  fingers.  The  finger  continues  to  spin  the  can, 
until  it  has  been  rotated  180®  into  a  new  orientation,  figure  8.8.  The 
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free  finger  is  then  replaced  on  the  can,  securing  the  grasp.  The  can  has 
been  completely  reoriented  in  the  grasp. 

In  the  third  routine,  the  hand  grasps  a  box  and  pushes  it  against  a  table, 
so  that  the  box  slides  through  the  fingers.  The  box  is  then  lifted  from 
the  table  and  the  free  finger  spins  it  180®.  The  hand  again  grasps  the 
box  and  again  forces  it  against  the  table,  repeating  the  same  procedure 
over  and  over  again,  as  shown  in  figure  8.9. 
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Figure  8.5:  The  Salisbury  robot  hand  holds  a  can,  as  in  the  example  in  the 
previous  chapter.  By  adjusting  the  squeezing  force  on  the  can,  the  robot  can 
either  secure  the  can  in  the  grasp  or  change  the  orientation  by  allowing  it  to  slip. 


CHAPTER  8.  IMPLEMENTATION 


94 


Figure  8.6;  This  is  a  map  shows  the  constraint  state  regions  as  a  function  of  the 
grasping  force  focus  Xg  yg  for  constant  grasp  force  magnitude  m,  s  0.7iV.  The 
current  position  in  the  map  is  shown  as  a  point  in  the  grasp  plane.  The  current 
constraint  state  is  [2,2,2],  a  fully  constrained  region  in  the  map.  However,  by 
moving  to  any  point  in  the  adjacent  [3,2,2]  region,  the  can  will  spin  between  the 
fineers. 
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Figure  8.7:  The  Salisbury  robot  hand  grasps  a  soda  caji.  The  magnitude  of  the 
grasping  force  is  0.7  N  and  the  grasp  force  focus  Xg  =  0.0cm  =  -0.8cm.  so 

that  the  can  is  secure  in  constraint  region  [2,2,2].  However,  by  moving  the  grasp 
force  focus  to  Xg  =  0.8cm  y,  =  -0.8cm,  the  can  will  spin  between  two  fingers. 
The  grasp  force  focus  is  then  moved  back  into  its  original  position  and  the  grasp 
is  again  secured. 
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Figure  8.8:  A  can  is  initially  held  securely  in  the  robot  hand.  The  grasp  force 
focus  shifts  from  its  current  stable  position  to  a  point  on  the  line  intersecting  the 
contact  points  made  by  the  first  and  third  fingers.  By  removing  the  second  finger, 
a  constraint  state  [1,4,1]  is  produced  and  by  using  the  free  finger  to  exert  a  force, 
the  can  rotates  in  the  grasp.  In  this  demonstration  the  finger  continues  to  spin 
the  can  until  it  has  rotated  it  180^.  Finally,  the  free  finger  is  returned  to  its  origin 
position  and  ’'f.  grasp  force  focus  is  moved  to  the  center  of  the  grasp. 
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Figure  8.9:  Slipping  motions  can  be  linked  to  form  a  sequence  of  motions  to 
reorient  a  part  within  the  hand.  In  this  case,  the  hand  pushes  a  box  against  the 
table,  forcing  the  box  through  the  grasp.  The  hand  then  lifts  the  box,  reduces 
the  grasping  force  magnitude,  shifts  grasp  force  focus  between  two  of  the  fingers, 
and  uses  the  free  finger  to  spin  the  box  180°.  The  hand  again  grasps  the  box  and 
forces  it  against  the  table  and  then  repeats  the  entire  process. 


Chapter  9 

Extensions  and  further 
reseeirch 


9.1  Introduction 

The  goal  of  this  thesb,  is  to  gain  a  basic  understanding  of  a  highly  dex¬ 
terous  type  of  manipulation  that  we  take  for  granted,  namely  controlled 
slip  manipulation.  An  analysis  was  performed  on  grasped  objects  to 
determine  which  ways  they  can  move  in  the  grasp.  The  set  of  possible 
motions  was  found  as  a  function  of  the  constraint  state  of  the  object, 
that  is,  the  number,  location,  orientation,  and  the  type  of  each  con¬ 
tact.  The  constraint  state  is  a  function  of  a  number  of  controllable 
variables,  such  as  the  grasp  force,  orientation,  and  stifhiess  of  the  lin¬ 
gers.  By  changing  the  grasp  force,  for  example,  the  constraint  state  of 
the  object  can  be  varied  to  allow  slipping  within  the  grasp. 

This  analysis  is  only  a  beginning  and  many  important  issues  have  yet 
to  be  addressed.  This  chapter  will  outline  some  extensions  to  the  gen¬ 
eral  problem  of  controlled  slip  manipulation.  It  is  hoped  that  these 
extensions  will  allow  this  type  of  manipulation  to  be  more  useful  and 
predictable,  thus  enhancing  the  dexterity  of  robot  hands. 
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9.2  Determining  permissible  motion 

When  a  hand  grasps  an  object,  there  are  only  so  many  ways  that  it  can 
move  in  a  grasp.  For  some  constraint  state,  the  space  of  permissible 
motions  of  the  object  is  quite  large,  which  makes  the  control  of  the 
motion  diflBcult.  However,  there  are  some  constraint  states  which  yield 
a  very  limited  range  of  permissible  motions,  so  that  the  motion  of  the 
object  can  be  determined  as  a  function  of  the  geometric  constraints 
alone.  With  the  current  algorithm,  a  desired  twist  is  specified  and  the 
constraint  state  which  allows  this  twist  is  determined.  Although  the 
algorithm  determines  the  constraint  state  which  provides  maximum 
constraint  while  still  permitting  the  desired  motion,  it  does  not  reason 
as  to  which  motions  would  be  more  advantageous  for  a  given  graisps  or 
particular  task.  What  would  be  more  useful  in  terms  of  controlled  slip 
manipulation,  would  be  to  determine,  perhaps,  using  a  set  of  heuris¬ 
tics,  a  small  set  of  permissible  motions,  which  may  be  easily  achieved 
through  slipping  motion.  To  accomplish  this,  using  the  current  analy¬ 
sis,  would  require  searching  a  six  dimensional  space  to  determine  the 
extent  to  which  the  permissible  twist  is  bounded.  This  may  in  fact  not 
be  too  diflBcult,  but  a  set  of  rules  may  quickly  and  efficiently  produce 
a  set  of  slipping  motions  the  robot  can  accomplish.  This  approach  is 
being  investigated  and  looks  promising  for  practical  implementation  on 
a  robot  hand. 


9.3  Determining  constraint  state 

In  tandem  with  the  problem  of  determining  set  of  slipping  motions 
which  can  be  easily  implemented  in  a  particuleir  grasp,  is  the  problem 
of  finding  the  desired  constraint.  With  the  present  analysis,  a  con¬ 
straint  state  map  is  generated  as  a  function  of  controllable  variables  on 
the  object.  To  find  a  desire  constraint  state,  the  entire  map  is  generated 
or  the  space  is  searched  until  the  values  of  grasp  force  are  found  which 
produce  the  desired  constraint  state.  Again,  a  set  of  heuristic  rules  may 
allow  the  robot  to  quickly  find  a  particular  constraint  state  within  a 
large  space,  so  that  the  slipping  manipulation  may  be  achieved  quickly. 
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These  rules  may  be  quite  simple,  since  there  are  some  constraint  states 
which  are  independent  of  some  of  the  controllable  variables.  For  exam¬ 
ple,  some  grasp  foci  always  produce  a  particular  constraint  state. 

9.4  Global  motion 

The  current  analysis  only  determines  the  set  of  permissible  twists  for 
small  finite  motions.  In  general,  to  reorient  an  object  within  a  hand, 
large  motions  need  to  be  considered.  In  order  to  study  global  motions, 
many  other  issues  must  be  addressed.  For  example,  the  size,  shape, 
location,  and  orientation  of  the  grasp  object,  the  robot  hand,  and  the 
surrounding  environment  must  be  known.  By  modeling  the  robot,  the 
object,  and  the  environment  as  polyhedral  solids  the  global  motion  of 
the  object  may  be  determined.  This  problem  is  not  as  difficult  as  the 
general  path  plaiming  problem,  since  the  path  is  given  as  the  particular 
twist,  only  magnitude  of  the  twist  needs  to  be  determined.  Therefore 
for  a  small  set  of  desired  twists,  their  magnitudes  can  be  found  and 
with  this  information  controlled  slip  trajectories  may  be  planned  and 
implemented  on  a  robot  hand. 


9.5  Sensory  feedback 

The  discussion  so  far  deals  with  slipping  motion  based  on  a  model  of 
the  robot,  object,  and  environment.  The  robot  has  no  feedback  on  ac¬ 
tual  object  position  or  contact  force.  In  order  to  performed  controlled 
slip  manipulations  accurately,  the  robot  must  know  the  position  of  the 
object  relative  to  the  grasp,  as  well  as,  the  location,  orientation,  and 
type  of  contact  which  exists  at  each  interface  between  the  manipulator 
and  the  object.  The  contact  information  may  be  determined  using  a 
force  and  tactile  sensing  surfeces  on  the  fingertips  of  the  robot  hand.  A 
contact  resolving  sensor  was  built  for  the  Salisbury  robot  hand,  which 
determines  the  location  of  a  contact  on  the  surface,  as  well  as,  the 
normal  and  tangent  force  at  the  interface.  Appendix  C  describes  this 
sensor  and  issues  involved  in  its  design.  A  sensor  like  this  is  important 


CHAPTER  9.  EXTENSIONS  AND  FURTHER  RESEARCH 


101 


in  controlled  slip  manipulation,  because  it  can  determine  the  location 
of  the  contacts  and,  using  a  model  of  the  fingertips,  determine  the 
contact  type.  However,  this  type  of  sensor  may  be  used  directly  to 
determine  contact  type.  Since  slipping  usually  coincides  with  high  fre¬ 
quency  vibrational  noise,  slipping  may  be  determined  by  looking  at  the 
high  frequency  components  of  the  force  signal.  Given  a  high  frequency 
force  signal,  together  with  the  mean  tangent  and  normal  force  readings, 
it  may  be  possible  to  determine  whether  the  slipping  was  translational 
or  rotational.  That  is,  given  a  high  frequency  vibration  which  suggests 
slip,  then 


\^cj ^  I  =»  C3 

\wcjnwcy  \  <  1  =>  C2 

where  is  a  point  contact  with  friction,  the  object  is  rotating  about 
the  contact  point,  and  C3  is  a  point  contact  without  friction,  the  object 
is  translating  at  the  contact. 

Determining  the  location  of  the  object  in  the  grasp  is  somewhat  more 
difficult.  For  some  class  of  objects  and  for  some  set  of  contacts,  the 
tactile  data  alone  may  be  enough  to  uniquely  determine  the  position 
of  the  object.  However,  tactile  sensing  alone  would  not,  for  example, 
resolve  the  location  of  a  cylinder  held  vertically  on  its  sides  by  the  fin¬ 
gertips  of  a  hand.  The  contacts  and  contact  forces  in  this  situation  are 
independent  of  the  vertical  position  of  the  cylinder.  Some  other  means 
than  tactile  sensing  would  be  necessary  to  determine  position.  Vision 
would  be  a  possible  solution,  except  that  resolving  object  location  at 
the  speeds  necessary  for  controlling  slipping  motion  is  currently  an  im¬ 
possible  task.  However,  vision  would  seem  to  be  the  eventual  solution, 
since  in  order  to  manipulate  objects  intelligently,  a  robot  would  have 
to  know  not  only  the  position  of  objects  within  the  grasp,  but  also  the 
position  of  objects  in  the  environment,  since  they  may  interact  with 
the  motion  of  the  grasped  object.  A  robot  could  both  avoid  external 
objects  and  use  them  to  exert  controlled  forces  on  the  object  within 
the  grasp. 
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9.6  Integrating  manipulation  techniques 

The  purpose  of  a  robot  is  to  interact  with  the  environment,  to  sense  its 
nature  and  to  affect  changes.  To  this  end,  higher  lever  plans  are  needed 
to  determine  global  actions,  as  well  as,  lower  level  strategies  which 
integrate  manipulation  techniques,  including  controlled  slip.  Methods 
must  be  developed  to  select  manipulation  strategies  to  accomplish  the 
lower  level  goals  as  set  by  the  higher  level  plans.  For  example,  to  invert 
a  cylinder  within  a  grasp,  a  robot  may  put  the  object  down,  reposition 
the  hand,  and  regrasp  the  object,  effectively  inverting  the  can  in  the 
grasp.  Or,  it  may,  using  control  slip,  spin  the  cylinder  between  two 
of  its  fingers,  and  thereby  accomplishing  the  same  feat.  In  spinning 
the  cylinder,  the  robot  could  use  gravity,  acceleration,  other  objects,  or 
other  fingers  to  reposition  the  object.  To  achieve  the  desired  goal  state, 
the  robot  may  have  to  perform  a  number  of  slipping  manipulations  in 
a  row.  This  suggests  some  sort  of  lower  level  slip  planning  based  on 
analysis  or  experience. 
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9.7  Conclusion 

This  thesis  presents  a  basic  analysis  of  controlled  slip  manipulation  and 
suggests  some  methods  of  implementing  slip  manipulation  on  robot 
hands.  Given  a  particular  grasp,  the  set  of  small  finite  motions  the 
object  can  undergo  within  the  grasp  was  determined.  The  set  of  per¬ 
missible  motions  within  a  grasp  was  found  to  depend  on  the  constraint 
state,  that  is  the  location  and  the  types  of  contacts  that  exist  between 
the  robot  and  the  object.  The  constraint  state  in  turn  depends  a  num¬ 
ber  of  controllable  variables,  such  as  grasping  force,  orientation,  finger 
stiffness,  and  other  variables.  By  controlling  the  constraint  state  and 
external  forces  which  act  on  the  grasped  object,  controlled  slipping 
motions  can  be  achieved.  Before  this  type  of  manipulation  can  be 
practically  implemented  in  robot  system,  more  efficient  ways  must  be 
found  to  analyze  grasps  and  actuate  motions.  This  thesis  does,  how¬ 
ever,  demonstrate  that  manipulating  objects  by  allowing  them  to  slip 
and  twist  at  the  fingertips  is  possible  for  robots  as  well  as  for  humans. 
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Appendix  A 


Coordinate  frames 


A.l  Introduction 

Objects,  fingers,  fingertips,  and  contact  points  can  all  described  relative 
to  different  coordinate  frames.  There  are  a  number  of  coordinate  frames 
used  in  this  thesis  and  any  point  in  space  may  be  described  with  respect 
to  any  or  all  the  coordinate  frames.  There  are  a  total  of  nineteen 
reference  frames.  These  are: 

OXY Z  Arbitrary  reference  frame 

O/iX^YiiZh  Hand  frame 

0/iiXf,jYf,jZf,j  Finger  frames 
Oi-Xt^Yt-Zti  Fingertip  frames 

OciXciYciZci  Contact  frames 

OgXgYgZg  Grasp  frame 

OoXoYoZo  Object  frame 

Descriptions  of  the  individual  reference  frames  are  given  in  the  following 
sections. 
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Zh 


Xk 


Figure  A.l:  Hand  frame 


A. 2  Hand  frame 

The  origin  of  the  hand  frame  is  centered  between  the  joints  of  the  first 
and  second  fingers  of  the  Salisbury  band.  The  x*axis  is  directed  out 
from  the  origin,  through  the  center  of  the  first  joint  of  the  second  finger, 
as  shown  in  figure  A.l.  The  z-axis  is  vertical,  normal  to  the  plane  of 
motion  of  the  first  phalange  of  the  first  finger.  The  y-suds  is  normal  to 
both  the  X  and  y  axes,  directed  away  from  the  wrist,  lying  in  the  plane 
of  the  motion  of  the  first  phalange  of  the  first  finger. 


A. 3  Finger  frames 

The  coordinate  frames  for  the  finger  phalanges  are  defined  the  same  way 
each  finger.  These  coordinate  frames  are  designated  O/^^X/jYf.jZfj, 
where  i  represents  the  finger  and  j  the  phalange.  The  finger 
frames  are  shown  in  figure  A.2.  The  origin  of  the  coordinate  frames  for 
the  phalanges  are  all  at  the  center  of  the  joints  and  the  y-axes  of  every 
frame  is  aligned  with  the  central  axis  of  the  phalange.  The  x  and  z 
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Figure  A.2:  Finger  frames 

aixes,  however,  are  defined  differently.  For  the  first  phalange,  the  z-axis 
is  the  axis  of  joint  rotation  and  the  x-axis  is  normal  to  the  y  and  z  axes 
in  the  direction  of  the  x-axis  of  the  hand  frame.  For  the  second  and 
third  finger  phalange  frames,  the  x-axis  is  the  axis  of  joint  rotation  and 
the  z-axis  is  vertical,  parallel  to  the  z-axis  of  the  hand  frame  when  the 
joints  are  in  the  zero  position. 

The  length  of  the  phalanges  the  fingers  of  the  Salisbury  robot  hand  are 
all  the  same.  Figure  A.3  shows  a  schematic  of  the  hand  illustrating  the 
length  of  the  phalanges  and  the  placement  of  the  fingers 


A. 4  Fingertip  frame 

The  fingertip  frames  OuXt^Yt^Zti  are  defined  the  same  way  for  each 
fingertip,  see  figure  A.4.  The  origin  of  the  fingertip  frame  is  the  center 
of  the  spherical  portion  of  the  fingertip.  The  z-axis  is  the  central  axis  of 
the  fingertip.  The  x-axis  is  parallel  to  the  x-axis  of  the  distal  phalange 
coordinate  frame. 
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Figure  A.3:  Phalange  length  and  finger  placement 
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Figure  A. 4:  Fingertip  frame 


APPENDIX  A.  COORDINATE  FRAMES 


no 


Figure  A.5:  Contact  frame 

A. 5  Contact  frames 

The  contact  frames  are  defined  in  terms  of  the  object  the  hand  is  grasp¬ 
ing.  The  origin  of  the  contact  frame  is  the  contact  point  between  the 
hand  and  the  object.  The  z-axis  is  the  surfaw^e  normal  at  the  contact 
point.  The  x  and  y  axes  lie  in  the  tangent  plane  of  the  surface.  For 
convenience,  the  x-axis  is  defined  to  be  parallel  to  the  x-axis  of  the 
hand  frame  or  to  lie  parallel  to  the  plane  described  by  the  x  and  y  axes 
of  the  hand  frame,  figure  A.5.  In  this  way  it  is  possible  to  generate  an 
unambiguous  definition  of  the  contact  frames. 


A. 6  Grasp  frame 

The  grasp  frame  is  defined  in  terms  of  the  contact  points.  The  origin 
of  the  grasp  frame  is  the  centroid  between  the  contact  points.  The  z- 
axis  is  normal  to  the  plane  described  by  the  three  contact  points.  The 
x-axis  is  defined  in  the  same  way  as  the  contact  frames.  That  is,  the 
x-axis  is  either  parallel  to  the  x-axis  of  the  hand  frame  or  parallel  to 
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Figure  A.6:  Grasp  frame 


the  plane  described  by  the  x  and  y  axes,  figure  A.6. 

A. 7  Object  frame 

The  origin  of  the  object  frame  is  the  located  at  the  centroid  of  the 
object.  The  axes  of  the  object  frame  are  arbitrary,  but  are  usually 
defined  along  the  symmetries  of  the  object  for  convenience. 


Appendix  B 

Stress  state  in  a  fingertip 


B.l  Introduction 

A  recent  entry  into  the  field  of  robotics  has  been  the  development 
of  robot  hands.  These  hands  typically  have  at  three  or  more  fingers 
with  three  or  more  joints  in  each  finger.  The  fingertips  are  usually 
covered  with  some  kind  of  compliant  material.  The  Salisbury  robot 
hand  had  hemispherical  polyurethane  fingertips,  similar  to  that  foimd 
on  rollerskate  wheels.  Although  most  work  has  be  done  investigating 
grsisping  and  manipulation  of  objects,  little  has  been  done  in  analyzing 
the  complex  mechanical  interaction  between  the  finger  and  the  grasped 
object.  As  a  first  attempt  in  addressing  this  problem,  I  will  examine 
the  stress  throughout  the  body  of  the  fingertip  in  contact  with  a  flat 
object. 


B.2  Problem  definition 

The  robot  fingertip  is  shown  schematically  in  figure  B.l,  touching  a  flat 
surface.  The  problem  will  be  to  find  the  stress  throughout  the  body 
of  the  fingertip.  The  fingertip  is  assumed  to  a  hemisphere  made  of 
a  homogeneous  elastic  material.  The  material  on  the  real  robot  is  a 
polyurethane  with  an  elastic  modulus  of  approximately  40,000  psi  and 
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Figure  B.l:  Robot  finger  in  contact  with  a  flat  surface 

a  Poisson’s  ratio  of  about  0.45.  For  simplicity  in  the  analysis,  however, 
particularly  in  the  finite  element  procedure,  the  poisson’s  ratio  will  be 
assumed  to  be  zero.  Since  the  elastic  modulus  is  small,  as  it  is  for 
most  elastomers,  a  nonlinear  analysis  would  seem  necessary;  however, 
for  this  analysis  I  will  assume  the  contact  force  is  small  1  lb.  This  force 
should  be  small  enough  so  that  displacements  within  the  body  will  not 
large  enough  to  warrant  a  complex  nonlinear  analysis. 


B.3  Analytic  solution 

The  analytic  solution  to  the  stresses  throughout  a  body  in  contact  will 
another  body  has  not  been  worked  out.  The  analytic  solution  for  the 
stresses  on  the  surface  of  a  body,  however,  has  be  developed  by  Hertz. 
He  assumed  the  two  bodies  were  solid  elliptic  disks,  each  possessing 
radius  of  R  and  R’.  Furthermore,  these  disks  were  in  contact  along  a 
common  axis  under  an  applied  load  F,  figure  B.2.  Hertz  deduced  that 
the  pressure  distribution  between  the  two  bodies  can  be  described  by 
a  semi-ellipsoid  of  pressure  constructed  over  the  surface  of  the  contact. 
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Figure  B.2:  Two  general  bodies  in  contact 


as  shown  in  figure  B.3 

The  pressure  distribution  is  given  by 


P(x,y)  =  p^yjl  -  x^/a'^  -  y^/lP 

(B.l) 

The  total  load,  therefore,  is  equal  to  the  volume  of  the 

semi-ellipsoid. 

p  2'KabPo 

3 

(B.2) 

Solving  for  P^ 

2xa4 

(B.3) 

where  a  and  b  are  given  by  Timosheko  [Lipson] 

J  3FA 

V  4(>l  +  B) 

(B.4) 

.  J  3FA 

”V4M  +  ^) 

(B.5) 
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F(®,y) 


Figiire  B.3:  Pressure  distribution  in  the  area  of  the  contact 
where 

A  -  (1  -  /jD/B,  +(1-  itVlIE,  (B.6) 

where  0.15 

Poisson’s  ratio 

ExEi  Modulii  of  elasticity 

I\/(^  -  ^)  +  (^  -  ^)^  +  2  (^  -  ^)  (^  -  cos(2^) 

V*  The  angle  between  the  planes  contacting  the  curvatures  \j  Ri  and 

1/fls 
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RxR'xR^R!^  Minimum  and  maximum  radii  of  curvature  of  the  ellip¬ 
soid  disks  at  the  point  of  contact 

m,  n  Constants  depending  on  B  —  AjB  -h  A 


In  the  case  of  the  contact  between  a  hemispherical  fingertip  and  a  flat 
rigid  plate,  equations  B.4  and  B.5  become 

a  =  6  =  0Fi2i  A/4  (B.7) 

and  the  maximum  pressure  is  given  by 

ft  =  (B.8) 

again  the  definition  of  A  is 


A  =  (1  -  +  (1  -  /*!)/£,  (B.9) 


If  the  flat  plate  is  assumed  to  be  rigid  (i.e.  E3  :>  Ej)  then  A  can  be 
approximated  as 

A  =  (1  -  iil)/E,  (B.IO) 

For  this  problem, 


F  =  11b 
El  =  40,  OOOpsi 
Ri  =  0.5in 

H  =  0.0 


and  therefore 


A  =  2.0  X  10-® 

-  1247.9 v^lb/in^ 
a  =  0.020  ^in 


Although  Hertz  only  solved  for  the  stress  in  the  are  of  the  contact,  H.R. 
Thomas  and  V.A.  Hoersch  computed  the  stress  within  the  body  along 
the  loaded  axis. 


o’rx  =  <ryy  = 


2o(l/Ri  +  I/R2) 

irA 


(1  +  ii){{zla)  cot~\z/a)  -  1)  + 


= 


— 2a(l/i2j  +  1/ R2) 


(^) 


2(a2  +  z^) 

(B.ll) 

(B.12) 


Also,  because  of  symmetry,  <Tx*,<ryy,  and  are  the  principle  stresses, 
and  therefore, 

Tyz  =s  Ty,  =  0  (B.13) 

A  plot  of  the  stress  gradients  due  to  the  contact  between  a  flat  rigid 
plate  and  a  spherical  flngertip  on  the  loaded  axis  is  shown  in  flgure  B.3 
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Figure  B.5;  Initial  finite  element  mesh 

B.4  Finite  element  solution 

The  finite  element  mesh  that  was  used  to  analyze  the  stress  is  shown 
in  figure  B.4.  The  figure  shows  the  finite  element  representation  of  one 
radian  "slice”  of  the  hemispherical  fingertip.  Axisymmetric  elements 
are  used  since  the  body  is  symmetric  around  the  axis  of  the  contact. 
Eight  node  elements  are  also  used,  since  they  more  accurately  model 
the  curvature  of  the  sphere.  The  contact,  however,  is  more  difficult  to 
model.  A  contact  element  could  be  used,  in  which  more  of  the  element 
is  subjected  to  a  force  as  the  element  deflects.  However,  since  the  radius 
of  contact  area  is  small,  0.019  in,  compared  to  the  elements  which  are 
0.050  in  on  a  side,  a  close  approximation  is  a  single  concentrated  load 
located  at  the  center  of  the  contact  area.  A  test  of  this  assumption 
and  the  other  approximations  will  be  whether  the  stresses  calculated 
by  the  finite  element  solution  match  those  of  the  analytic. 

To  test  the  validity  of  the  mesh,  the  stresses  between  identical  nodes  on 
adjacent  elements  are  compared.  Figure  B.4  shows  the  finite  element 
mesh.  The  vertical  line  drawn  on  the  figure  was  the  line  on  which 
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Figure  B.6:  Line  on  which  nodal  stress  are  compared 

stresses  were  calculated.  Figure  B.4,  B.4,  and  B.4  shows  the  stress  on 
the  line  calculated  at  nodes  on  adjacent  elements.  The  two  curves  in 
each  figure  represent  the  nodal  stress  on  the  two  columns  of  elements 
(i.e.  elements  1  through  10  and  elements  11  through  20).  In  every 
figure,  the  stresses  calculated  at  the  same  nodes  for  adjacent  elements 
are  inconsistent  for  values  of  z  less  than  0.25  inches.  For  example, 
figure  B.4  shows  the  stress  in  the  y  direction,  (Tyy.  At  z=0.075  in  the 
stress  calculated  at  the  same  node  vary  from  -20  psi  in  element  12  to 
-100  psi  in  element  2.  On  the  other  hand,  for  values  of  z  greater  than 
0.35  in,  the  variation  of  stress  between  elements  is  negligible,  as  can  be 
seen  in  figure  B.4.  It  is  also  interesting  to  note  that  the  stress  jumps 
for  (Tyy  occur  between  elements  in  adjacent  colunms,  while  for  cr„  they 
occtir  between  elements  in  the  same  column.  For  Tyg,  the  stress  jumps 
between  adjacent  columns  and  adjacent  rows. 

It  is  clear  from  this  analysis,  that  for  an  accurate  prediction  of  stress  in 
the  body,  this  mesh  is  too  coarse.  Variations  in  stresses  calculated  at 
the  same  nodes  were  greater  that  100%,  for  radial  distances  of  less  that 
0.25  in  from  the  contact  region.  Figure  B.4  shows  a  three  dimensional 
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Figure  B.ll:  Stress  profile  on  hemispherical  body 


plot  of  stress  with  the  y  and  z  coordinates  plotted  in  the  plane  and 
the  stress  cTyy  plotted  on  the  vertical  axis.  Here  it  is  easy  to  see  the 
inconsistencies  in  the  stress  between  elements  Therefore,  for  an  accurate 
calculation  of  the  stress  within  the  body  of  the  fingertip,  a  new  more 
refined  mesh  must  be  constructed. 

B.4.1  Refined  mesh 

To  solve  the  problem  of  nodal  stress  inconsistencies,  a  new  refined  mesh 
is  constructed.  Figure  B.4.1  shows  the  region  in  which  the  mesh  need 
to  be  finer.  In  figxire  B.4.1,  a  new  refined  mesh  is  constructed  in  the 
specified  region  and  figure  B.4.1  shows  an  expand  view  of  that  region. 
As  before,  to  test  the  validity  of  this  new  mesh,  I  compared  the  stress 
along  a  single  line,  as  shown  in  figure  B.4.1.  The  stress  calculated  at  the 
nodal  points  between  adjacent  elements,  in  this  mesh  shown  almost  no 
inconsistencies  for  values  of  z  greater  than  0.02  in,  as  shown  in  figures 
B.4.1  and  B.4.1.  This  is  almost  an  order  of  magnitude  improvement 
over  the  previous  case.  Figure  B.4.1  shows  the  same  three  dimensional 
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Figure  B.12:  Region  of  mesh  refinement 

stress  profile.  Again,  the  y  and  z  coordinates  are  plotted  in  the  plane, 
and  the  (Tyy  is  plotted  on  the  vertical  axis.  As  before,  the  plot  shows 
stress  jumps  at  small  radial  distances  from  the  origin  of  contact.  How¬ 
ever,  if  these  few  values  of  stress  are  ignored,  the  resultant  stress  field 
is  continuous  along  the  line.  Another  test  for  the  accuracy  of  the  finite 
element  calculations  is  to  compare  the  results  with  an  analytic  solution. 


B.5  Finite  element  vs.  analytic 

To  compare  the  finite  element  solution  against  the  analytic  solution, 
only  the  stresses  on  the  z  axis  can  be  considered.  Figure  B.5,  B.5,  and 
B.5  show  the  stresses  <Tyy,  <7„,  and  respectively  9S  calculated  by 
the  finite  element  solution  and  the  analytic  solution.  The  figures  show 
an  excellent  correlation  between  the  analytic  and  the  finite  element 
solution,  particularly  for  values  of  z  greater  than  0.04  in.  Ignoring 
these  stress  calculations  near  the  origin,  a  accurate  representation  of 
the  stress  within  the  entire  body  of  the  fingertip  can  be  obtained. 
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Figure  B.13:  Refined  mesh 


Figure  B.14:  Expanded  view  of  the  new  refined  region 
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:>IXB.  STRESS  ST. 


Figure  B.15:  Line  on  which  nodal  stresses  are  compared 


igure  B.16:  Consistency  in  calculated  nodal  stresses:  <Tyy 
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Figure  B.19:  Agreement  between  the  finite  element  and  analytic  solutions: 
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Figure  B.20:  Agreement  between  the  finite  element  and  analytic  solutions: 
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Figtire  B.21:  Agreement  between  the  finite  element  and  analytic  solutions; 


B.6  Solution 

As  a  result  of  the  consistency  in  calculated  stress  between  the  ele¬ 
ment  and  because  of  the  agreement  between  the  analytic  and  the  finite 
element  solution,  the  modified  mesh  is  assumed  to  be  sufficient  for  cal¬ 
culated  stresses  throughout  the  body  of  the  hemispherical  fingertip. 
Figures  B.7,  B.7,  and  B.7  show  the  stress  profiles  through  the  body  of 
the  hemisphere.  From  the  analysis,  these  stresses  can  be  taken  aa  an 
accurate  representation  of  the  true  stress  in  the  fingertip. 


B.7  Conclusion 

In  this  analysis,  a  finite  element  method  was  developed  to  accurately 
predict  the  stresses  in  the  body  of  a  hemispherical  fingertip  in  contact 
with  a  flat  object  (for  relatively  small  forces).  This  information  can 
be  useful,  Tor  example,  in  determining  the  placement  of  subcutaneous 
force  and  tactile  sensors. 
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Appendix  C 
Fingertip  sensor 


C.l  Introduction 

A  contzict  resolving  fingertip  sensor  was  developed  which  determines 
the  location  of  a  contact,  as  well  as  the  direction  and  magnitude  of 
the  force  at  the  contact  point.  The  sensor  is  based  on  silicon  strain 
gauges  on  steel  flexures,  which  measure  the  forces  and  moments  at 
the  center  of  the  shell.  From  these  force  readings,  it  is  possible  to 
determine  the  direction  of  the  force  vector  acting  on  the  shell  of  the 
fingertip.  Using  the  moment  readings,  the  location  of  the  force  vector 
can  be  determined.  The  intersection  of  the  force  vector  with  the  outer 
surface  of  the  fingertip  yields  two  points.  One  point  corresponds  to  a 
pulling  on  the  surface  of  the  fingertip.  The  other  point  corresponds 
to  a  pushing  force  on  the  sensor.  Assuming  the  fingertip  only  pushes 
against  surfaces,  one  of  the  points  is  immediately  eliminated.  This 
leaves  a  single  point,  the  contact  point,  through  which  the  force  is 
acting.  Since  the  sensor  resolves  the  components  of  the  force,  it  can 
determine  the  magnitude  and  direction  of  the  force  at  the  point  contact 
as  well. 
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C.2  Theory 

The  theories  presented  in  this  section  were  developed  by  [Salisbury]. 
Suppose  a  force  F  =  /y,  fz]  is  acting  through  a  point  Xc  =  [xc,  j/c,  ^e] 

on  a  convex  surface  5.  The  convex  surface  S  is  defined  as  a  smooth 
surface  in  such  that  for  every  pair  of  points  pi  and  pj  in  S,  the 
line  mpi  +  (1  —  m)p2  for  m  €  3?  intersects  5  in  only  two  points  pi  and 
P2-  Also  for  any  two  points  pi  and  p2  with  contact  normals  ni  and  nj 
respectively,  the  dot  products  of  the  vector  Pi  —  pi  and  the  normals 
n\  and  have  different  sign.  This  condition  allows  the  unambiguous 
resolution  of  the  location  of  the  force  on  the  surface  S. 

Let  OXYZ  be  some  reference  frame  so  that  every  point  in  S  can  be 
defined  relative  to  OXYZ.  The  wrench  in  terms  of  screw  coordinates 
defined  relative  to  OXY Z  due  to  the  force  F  acting  through  the  point 
Xc  is 

1  r  fx 

W2  fy 

ti>3  _  ft 

Wa  ~  -Zcfy  +  ycfz 
1^5  ■2’c/x  ^eft 

L  -yc/x  +  Xjy 

Now  we  wish  to  describe  the  wrench  w  in  terms  of  the  wrench  axis,  the 
pitch,  and  the  magnitude.  The  direction  of  the  wrench  axis  A  is 

■  W-i 

dx  =  (C.2) 

W3 

and  the  point  on  the  wrench  axis  nearest  the  reference  frame  is 

’  Xo  j  ’  W2W3  —  WjWi  ' 

Xo  =  Vo  =  —  W3W4  -  wiwe  ,  (C.3) 

_  Zo  ^  WiWs  —  'W^W2 

where  m  is  the  magnitude  of  the  wrench  and  also  the  magnitude  of  the 
force  _ 
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Any  point  along  the  wrench  axis  can  be  described  by 

x=Xo  +  AdA.  (C.5) 

The  intersection  of  the  line  in  equation  C.5  with  the  surface  S  yields  one 
or  two  points.  If  the  intersection  yields  a  single  point  then  the  location 
of  the  force  on  the  surface  is  immediately  determined.  However,  if 
the  intersection  produces  two  points,  the  location  of  the  contact  can 
still  be  found.  Suppose  the  two  points  of  intersection  are  Xi  and  X2 
with  normals  nj  and  nj.  Take  the  dot  product  of  the  force  F  with 
the  normals  n\  and  n^.  The  point  associated  with  the  normal  which 
produces  a  negative  value  of  the  dot  product  is  the  contact  point. 

Therefore  it  is  possible  to  determine  the  point  of  contact  Xc  from  the 
wrench  w.  If  it  were  possible  to  measure  the  wrench  w,  then  the 
location,  direction  and  magnitude  of  a  force  exerted  through  a  point 
contact  on  a  surface  S  can  be  found.  This  is  precisely  the  concept 
behind  the  contact  resolving  fingertip  sensor. 


C.3  Design 

The  schematic  diagram  of  the  fingertip  sensor  is  shown  in  figure  C.l. 
The  fingertip  has  a  hemispherical  top  and  a  cylindrical  skirt  which  bolt 
onto  a  loadcell.  The  loadcell  is  in  the  form  of  a  maltese  cross  and 
is  design  to  resolve  all  six  components  of  the  wrench,  that  is,  forces 
and  moments  in  three  cartesian  directions.  Small  semiconductor  strain 
gauges  are  mounted  on  the  legs  of  the  cross.  These  gauges  measure 
strain  which  is  proportional  to  the  bending  moment  in  the  beam  which 
is,  in  turn,  proportional  to  the  forces  and  moments.  The  next  section 
describes  some  critical  issues  in  the  design  of  the  loadcell. 


APPENDIX  C.  FINGERTIP  SENSOR 


136 


20  duiometei 
polyuretlxaae 
molded  eoTct 


Stainleu  iteel 
Hemupherieftl  cover 


Steinleu  (teel 
Cyliadrieel  tkirt 


ACTUAL  SIZE 


17-4  PH  lUialeu  >teel 
6-ucu  loedcell  with 
16  soon  lemieonductot 
(tiein  gengee 


Right  eagle 
polyarethaae 
molded  ittaia  relief 


O.lOOia  diameter  cable 
Polyarethaae  jacket 

38  AWG  iteel  braided  iheild 
Teflon  tape 
8  twieted  pain  - 


38  AWG  (7/48)  S.P.Oaraflex  9 
O.OOSia  teflon  wailed  pain 


Polyarethaae 
molded  itrain  relief 


Sabmiaiatare  locldag 
19  condactor  Hiroce  conaector 


Figure  C.l:  The  fingertip  sensor  can  determine  the  magnitude,  the  location, 
and  the  direction  of  a  force  applied  through  a  point  contact  on  its  surface.  Small 
semiconductor  strain  gauges  mounted  on  small  steel  flexure  are  used  to  resolve  the 
forces  and  moments  on  the  fingertip  shell. 


APPENDIX  C.  FINGERTIP  SENSOR 


137 


C.4  Design  of  loadcell 

C.4.1  Introduction 

The  loadcell  for  the  force  and  tactile  sensing  fingertip  must  be  able  to 
resolve  all  six  components  of  forces  and  moments  applied  to  it.  In  order 
to  properly  design  the  loadcell,  it  is  necessary  to  determine  the  forces 
and  moments  under  which  may  act  on  the  loadcell,  as  well  as  the  state 
of  stress  in  each  of  its  members.  The  fingertip  may  be  subject  to  an 
infinite  variety  of  forces.  However,  instead  of  examining  all  the  possible 
forces,  we  will  consider  only  a  small  set  of  forces  which  produce  extreme 
values  of  force  and  moment  on  the  loadcell.  This  set  of  forces  is  shown 
in  figure  C.2.  In  the  next  sections,  the  forces  and  moments  on  the 
loadcell  resulting  from  the  externally  applied  loads  will  be  determined, 
along  with  the  associated  stress  states  in  the  flexures  of  the  loadcell. 

The  analysis  of  stress  states  is  only  part  of  the  design  procedure.  The 
design  must  also  take  into  account  the  manufacture,  assembly,  and 
gauging  procedures,  in  order  to  minimize  cost  and  maximize  efficiency. 
From  this  analysis,  the  design  and  dimensions  of  the  loadcell  will  be 
determined. 

C.4.2  Fingertip  sensor  dimensions 

Figure  C.3  illustrates  and  lists  the  symbolic  values  for  the  dimensions 
of  the  fingertip  and  the  loadcell.  These  values  will  be  used  through¬ 
out  the  analysis  when  calculating  stress,  strain,  and  deformation.  The 
constrained  values  are 
R  =  0.406in 
H  =  0.813in 
Li  =  O.OTOin 
Lj,  =  O.lOOin 
Hi  =  0.633in 

and  the  values  yet  to  be  determined  are 
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Figure  C.2:  Forces  and  moments  are  generated  at  the  center  of  the  sensor  as  a 
function  of  the  externally  applied  load.  The  set  of  external  loads  which  produce 
maximum  internal  forces  and  moments  are  listed  above.  Each  of  these  cases  will  be 
considered  individually  and  the  resulting  forces  and  moments  will  be  calculated, 
as  wed  as  the  stresses  they  create  on  the  load  bearing  members. 
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Figure  C.3:  The  dimensions  for  the  loadcell  and  the  fingertip  illustrated  above 
will  be  used  through  the  analysis  on  the  fingertip.  From  the  analysis,  optimal 
values  for  the  dimensions  listed  will  be  found  to  maximize  fingertip  performance 
and  durability. 


C.4.3  Mechanical  properties 

The  loadcell  is  to  be  machined  from  17-4PH  stainless  steel.  This  par¬ 
ticular  type  of  steel  was  chosen  because  its  coefficient  of  thermal  expan¬ 
sion  matches  that  of  the  semiconductor  strain  gauges.  A  similar  value 
of  thermal  expansion  is  desired  since  both  the  gauges  and  sensor  are 
heated  during  the  bonding  process  and  cooled,  the  same  contraction 
rates  will  not  introduce  an  offset  strains  in  the  gauges. 

The  mechanical  properties  of  the  17-4PH  stainless  steel  are: 
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E  =  28.5  X  10«lb/in* 

G  =  10.6  X  10®lb/in^ 

(Ty  =  80, 000  -  100,  OOOlb/in^ 
€y  =  cTy/E  =  3157  li  strain 


The  mechanical  properties  of  the  silicon  strain  gauges  are: 
Maximum  strain  =  3000  n  strain 


C.4.4  General  cantilever  beam  problem 

The  analysis  of  the  cantilever  beam  will  be  used  throughout  the  anal¬ 
ysis  of  the  loadcell,  so  the  general  problem  is  stated  here  along  with 
the  standard  equations  of  load,  shear  force,  bending  moment,  and  de¬ 
formation. 

A  cantilever  beam  is  shown  in  figure  C.4.  It  is  subject  to  a  force  F  and 
a  moment  M  at  its  end  point.  The  deflection  of  the  end  point  is  given 
by  6  and  the  angle  from  the  horizontal  at  the  end  point  is  denoted  by 

<i>. 

Bending  moment 

The  load,  the  shear  force,  and  the  bending  moment  along  the  beam  are 
given  by 

Load: 

q{x)  =  {-FL  -  M)(x)_j  -H  F(x)_i  -t-  M(x  F{x  -  I)_i  (C.6) 

Shear  force: 
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v{x)  =  {FL  +  A/)(x)_i  -  F{x)°  -  M{x  -  I)_i  +  F{x  -  L)°  (C.7) 

Bending  moment: 

Miix)  =  i-FL  -  M){xf  +  F{xY  +  M{x  -  L)°  -  F{x  -  (C.8) 


Deformation 


The  displacement  of  the  end  of  the  beam  is  given  by 


^  FL^  ML^ 

^  “  3^7  2EI 

where  7  is  the  area  moment  of  inertia  given  by 


and  the  angle  (f>  is 

,  FL^  ,  ML 
^  2EI  El 


(C.9) 


(C.IO) 


Stress  and  strain 

In  general,  the  stress  due  to  a  bending  moment  M(,  is 

Myy 

where  7  is  the  area  moment  of  inertia  and  y  is  the  distance  from  the 
point  of  zero  stress  to  point  at  which  the  stress  is  measured.  In  the  case 
of  the  rectangular  cantilever  beam,  the  point  of  zero  stress  coincides 
with  the  center  of  the  beam.  Therefore,  the  maximum  stress  due  to 
bending  is  located  on  the  outer  surface  of  the  beam.  That  is,  when 
y  =  hf2,  the  stress  is 

h^w 


(C.12) 
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C.4.5  Case  1 

Assumptions 

Assume  a  force  is  applied  to  the  fingertip  on  the  top  of  the  hemisphere, 
figure  C.5.  The  force  is  applied  vertically  along  the  major  axis  of  the 
sensor.  By  symmetry,  fingertip  shell  will  be  displaced  vertically,  no 
horizontal  motion  or  rotation  will  occur.  Also  by  symmetry,  the  forces 
will  by  applied  equally  to  each  leg  of  the  cross. 


Analysis 


A  single  leg  of  the  cross  is  shown  in  figure  C.6.  By  of  symmetry,  each 
leg  of  the  cross  receives  an  equal  force,  F/4,  and  undergoes  an  equal 
displacement  S,  and  angle  <f>  =  Q.  That  is,  given 


(F/4)If  ML,  _ 
2EI  El 


(C.13) 


where 


then  the  moment  will  be 


(C.14) 


Therefore  the  loading  is 


(C.15) 


q{x)  =  -FL//8(x)_3  +  F/4(x)_i  (C.16) 

v{x)  =  FL,IB{x).^  -  F/4(x)°  (C.17) 

A/fc(x)  =  -FLi/8{x)°  +  F/4(x)'  (C.18) 

(C.19) 


The  maximum  bending  moment  occurs  at  x  =  0  and  Mi,  =  —FL/B. 
Therefore,  the  maximum  stress  in  the  beam  under  this  particular  load¬ 
ing  is 

_  3FF 


(C.20) 
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C.4.6  Case  2 

Assumptions 

A  hori2ontal  force  is  applied  to  the  fingertip,  in  line  with  the  loadcell, 
as  shown  in  figure  C.7.  The  force  is  evenly  distributed  on  either  side 
of  the  loadcell,  as  a  result  no  rotation  occurs.  Since,  by  symmetry, 
both  sides  of  the  loadcell  will  behave  the  same  way,  one  half  of  the 
loadcell  will  be  analyzed  and  is  depicted  in  figure  C.8.  It  is  assumed 
the  compressive  displacements  are  negligible,  therefore,  both  the  legs 
parallel  to  the  applied  force  and  the  phalanges  are  assumed  to  be  rigid. 
In  addition,  a  cantilever  beam  model  is  assumed  for  both  the  legs  and 
the  phalanges. 

Analysis 

The  two  phalanges  and  the  leg  of  the  cross  on  each  side  of  the  load¬ 
cell  are  displaced  by  the  same  amount  6,  figure  C.8.  The  ends  of  the 
phalanges  and  the  legs  do  not  rotate,  therefore  =  0  and  by  equation 
C.IO 


Mi  =  -FiLi/2 

(C.21) 

Substitute  into  equation  C.9,  the  displacement  is 

s 

12EiJi 

(C.22) 

and  the  force 

(C.23) 

Since  the  force  F  is  evenly  distributed  on  each  side  of  the  loadcell,  the 
Slim  of  the  force  on  the  each  flexure  is  F/2,  that  is 

F/2  =  Fj,  +  Fi  +  Fj, 

(C.24) 

Substitute  C.22  into  C.24  yields 

.  Ewfh,  ,  Ew^h„' 

LJ  +  if  +  J 

(C.25) 
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Solve  for  6 


8  = 


FLUl 


2£(w?A,iJ  +  2wJ/i,i?) 
The  force  and  the  moment  on  the  leg  of  the  cross  is 


Fi  = 


rvfhiLl 


^wfhtLl  +  2wlhpLfJ  2 


Ml 


^ 

\wfhiLl- 


tofhiLl 


_ \  F^ 

+  2wlhj,Lf)  4 


and  the  force  and  moment  on  the  phalanges  are 

(  wlh^f  \  F 
'  \wih^i  +  2wfh,Ll)  2 


M, 


(  o’lh.Lf  \ 

\wfhpLf  +  2wfhiLV  4 


Finally,  the  maximum  stress  on  the  leg  of  the  cross  is 

ZwiLlLiF 

2{wfhiLl  +  2wfh,Lf) 

and  the  naaximum  stress  on  the  phalange  is 


_  ZwpLfLpF 
2{wfh^f  A  2wfhiLl) 


(C.26) 

(C.27) 

(C.28) 

(C.29) 

(C.30) 

(C.31) 

(C.32) 

(C.33) 

(C.34) 


C.4.7  Case  3 

Assumptions 

A  force  is  applied  at  the  base  of  the  fingertip  shell  in  the  horizontal 
direction  as  shown  in  figure  C.9.  This  particular  case  produces  the 
largest  stress  on  the  members  of  the  loadcell.  The  force  applied  in  this 
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orientation  and  position  will  therefore  be  limiting  force  in  the  design 
of  the  loadcell.  It  is  assumed  the  loadcell  undergoes  a  rotation  and 
deformation  as  illustrated  in  figure  C.IO.  The  phalanges  parallel  to  the 
applied  force  are  assumed  to  undergo  rigid  rotation.  The  phalanges 
perpendicular  to  the  applied  force  are  subject  to  a  moment,  but  for 
this  analysis  these  phalanges  are  also  assumed  rigid.  Therefore  the 
entire  outer  structure  of  the  cross  undergoes  a  rigid  rotation  (j).  As  in 
the  previous  case,  the  legs  perpendicular  to  the  external  force  undergo 
a  displacement,  however,  since  the  major  stress  components  are  on  the 
legs  parallel  to  the  external  force,  these  displacements  are  assumed  to 
be  negligible. 


Analysis 

A  free  body  diagram  of  the  cross  is  shown  in  figure  C.ll.  The  de¬ 
formation  from  the  force  F  is  assumed  small  in  comparison  with  that 
from  the  moment  FHx^  The  two  legs  perpendicular  to  the  applied  force 
undergo  a  rotational  twist  subject  to  a  moment  and  the  two  legs 
parallel  to  the  appUed  force  bend  under  a  moment  M\  and  a  force  F\ . 
By  equilibrium, 

FHx  =  2FxLi  -  2Mi  -  2M2  (C.35) 

A  diagram  of  a  single  leg  undergoing  bending  is  shown  in  figure  C.12. 
By  geometric  compatibility,  the  displacement  equals  the  length  of  the 
beam  times  the  angle,  that  is,  assuming  angles  are  small. 

S  =  Li<i>  (C.36) 

Substitute  into  the  equations  for  displacement  and  angle  and  solve  for 
the  moment  Mi  yields 

Ml  =  -FiL,  (C.37) 


0  = 


ISFiLj 

Ewihf 


(C.38) 


and  the  angle  <f> 
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Figure  C.13  shows  a  diagrsun  of  a  beam  under  the  torsional  load,  and 
the  equation  for  angular  rotation  as  a  function  of  moment  is 


-M2L{ 

C2Gkiwl 


(C.39) 


for  W\  <  hi  and  Cj  =  0.170  approx.  Equate  equations  C.38  and  C.39 
and  solve  for  the  moment  M2 


Now  substitute  equations  C.37  and  C.40  into  C.35 
FH,  =  2F,L,  -  -  2 


(C.40) 


and  solve  for  the  force  Fi 


Fi  = 


4Xj  + 


FHi 

^^Lic^Owf 


(C.41) 


{C.42) 


Figure  C.ll  shows  a  single  leg  of  the  cross  under  the  applied  loads.  By 
equilibrium  the  sum  of  the  moments  at  the  root  of  the  leg  are  zero. 
That  is, 

-  Fill  +  Ml  +  M'  =  0  (C.43) 

and  using  equation  C.37 

M'  =  2FiL(  (C.44) 

and  is  also  the  maximum  bending  moment  under  which  the  beam  is 
subject.  Substituting  equation  C.42  into  the  above  C.44  yields. 

a  »  F^  H 1  IjI  .  J  C  \ 


Mfnax  ~~ 


c  r  ,  ISLjcjGtu? 

+  ~  Efr 


(C.45) 


Therefore  the  maximum  stress  is 


SFHiL, 


(C.46) 
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C.4.8  Case  4 

The  fingertip  may  also  be  subject  to  a  tangent  force  as  shown  in  figure 
C.14.  This  case,  however,  is  identical  to  case  3,  except  for  the  length 
of  the  moment  arm.  In  this  case  it  \a  R  +  H  —  Hi  instead  of  Hi  and 
the  maximum  stress  is  therefore 


^max  — 


3F{R  +  H-Hi)L, 


Li  + 


Eh] 


(C.47) 


C.4.9  Case  5 

Assumptions 

Figure  C.14  shows  a  force  applied  to  the  fingertip,  tangent  to  the  sur¬ 
face.  It  is  assumed  this  force  is  on  the  same  plane  as  the  loadcell, 
therefore  stress  is  only  induced  on  the  sides  of  the  flexures.  In  this 
case,  the  stress  due  to  the  force  and  the  stress  from  the  induced  mo¬ 
ment  superimpose,  so  that  each  case  must  be  considered  in  order  to 
find  the  maximum  stress  on  the  members.  First  we  will  determine  the 
maximum  stress  due  to  the  moment  FR  and  then  add  the  maximum 
stress  determined  from  case  2.  Figure  C.15  shows  the  loadcell  rotated 
as  a  result  of  an  applied  moment  M.  It  is  eissumed  all  the  phalanges 
are  rigid  and  that  the  entire  outer  structure  of  the  loadcell  undergoes 
a  rotation  <ft. 

Analysis 

A  single  leg  of  the  cross  is  shown  in  figure  C.  1 1 .  It  is  subject  to  a  force  F, 
smd  a  moment  Af,-  applied  at  its  endpoint.  By  geometric  compatibility 
the  displacement 

6  =  <(>Li  (C.48) 

and  using  equation  C.9,  the  moment  can  be  found  in  terms  of  the  force 

Mi  =  Fill  (C.49) 
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By  symmetry,  each  leg  of  cross  is  under  the  identical  loading  as  shown 
in  figure  C.15.  The  sum  of  the  moments  must  be  zero,  therefore. 


FR  =  AFiL,  +  AFiLi 

(C.50) 

and 

F.=  ^ 

‘  8L, 

(C.51) 

and 

M..=  — 

(C.52) 

A  free  body  diagram  of  a  single  leg  is  shown  in  figure  C.ll,  sum  the 
moments  at  the  base  of  the  cantilever 


A//  = 


FR 


(C.53) 


which  is  the  maximum  bending  moment  on  the  beam.  Substitute  into 
the  equation  for  the  stress,  gives, 


<T  =s 


3FR 

2hiwf 


(C.54) 


Now  we  must  also  consider  the  stress  from  the  tangent  force 

^wiLlLiF 


<Tl  = 


2{wfhiLl  +  2wj,hj,Lf) 

The  sum,  therefore,  will  be  the  maximum  stress  on  the  beam 


(C.55) 


<Ti  = 


3R 


SwiLlLi 


[2hiwf  2{wfhiLl  +  2w^hj,Lf)\ 


(C.56) 


C.4.10  Case  6 

Suppose  the  fingertip  is  subject  to  tangent  force  on  the  outer  radius  of 
the  hemisphere.  As  in  the  previous  case,  we  must  again  consider  the 
superposition  of  two  stresses,  since  the  stress  due  to  the  induce  moment 
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and  the  vertical  force  superimpose  on  one  of  the  legs  of  the  cross.  From 
the  analysis  in  the  first  case  the  stress  was 

3FL 


^max  — 


4wihf 

and  from  case  2,  substitute  R  for  Hi  yields 

3F{R  +  H-Hi)Li 


wihf 


T  1  SLiciGwf 


(C.57) 


(C.58) 


The  majdmum  stress  in  this  case  is  therefore 


3L 


+ 


3RLt 


Wlhj 

C.4.11  Maximum  stress 


r  .  9LiC2Gwf 
Eh'f  ‘ 


(C.59) 


The  maximum  stresses  under  the  different  loading  modes  of  all  six  case 
are  listed  below 


^max 

^max 


3FL 


7(wpifLf+2wfhiL^) 
3FHt  Lt 


vnhf 


SFj 

R+H-ffi)L, 

wthf 

r, , 

ssf 

(C.60) 


—  f  3^  4.  3w,LlLt  1  p 
2{wpIi^+2wpi^Tf)  j 


3L 

4iuik^ 


wihf 


3RL, 


iLic^Gvf 
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Figure  C.4:  Cantilever  beam  subject  to  a  force  F  and  a  moment  M  at  its  end 
point. 


Figiire  C.5:  Force  applied  on  the  top  of  the  hemisphere  along  the  axis  of  the 
sensor 


\ 

- 

rf» 

r-Fl^8 

s 


Figure  C.6:  Leg  of  the  loadcell  under  a  symmetrically  applied  vertical  load 
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Figure  C.7;  A  horizontal  force  is  applied  to  the  outside  of  the  sensor  in  line  with 
the  inner  loadcell 


Figure  C.8:  The  members  under  compressive  stress  are  assumed  to  be  rigid  and 
since  the  force  is  applied  symmetrically  to  the  loadcell  it  is  assumed  to  move  rigidly 
and  not  rotate. 
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Figure  C.9: 


Horizontal  force  applied  to  the  lowest  portion  of  the  fingertip  shell 


Figure  C.IO:  The  outer  phalanges  are  all  assumed  to  be  rigid.  The  major  stress 
components  are  located  on  the  upper  and  lower  surfaces  on  the  legs  parallel  to  the 
externally  applied  force  at  the  cantilevered  end. 
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Figure  C.ll:  The  legs  parallel  to  the  applied  force  are  subject  to  both  a  force 
and  a  moment  at  their  end  points,  while  the  legs  perpendicular  to  the  force  are 
only  subject  to  a  pure  torsional  moment. 


Figure  C.12:  The  legs  parallel  to  the  applied  force  undergo  a  rotation  at  the  end 
point  of  4>  and  a  displacement  of  <i>Li,  under  an  applied  force  F\  and  a  moment 
Ml. 
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Figure  C.13:  The  leg  perpendicular  to  the  applied  external  force  twists  to  an 
angle  <t>  subject  to  an  applied  moment  M2. 


Figure  C.14;  A  horizontal  force  applied  tangent  to  the  surface  of  the  fingertip 
in  the  same  plane  as  the  loadcell 
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Figure  C.15:  The  outer  phalanges  are  all  assumed  to  be  rigid  and  rotate  rigidly 
to  an  angle  0  under  and  applied  moment  M.  The  major  stress  components  are 
therefore  located  on  sides  of  the  legs  of  the  cross 


Appendix  D 


Slip  analysis  software 


D.l  Introduction 

The  software  was  not  included  just  to  add  bulk  to  this  master’s  thesis. 
In  fact,  the  construction  of  the  LISP  functions  along  with  the  documen¬ 
tation  are  similar  to  the  development  of  the  chapters  in  the  thesis.  The 
high  level  functions  are  Ibted  at  the  beginning  of  every  section,  while 
the  lower  level  supporting  function  are  listed  in  a  separate  section,  one 
for  each  high  level  function. 

D.2  GRASP 
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Mode: LISP;  Base: 10;  Syntax:  Common-lisp;  Paclcage:  USER 


FILE;  CONSTRAINT. LISP 

This  file  contains  functions  which  analysis  the  relationship  between  a  grasped  obj 

and  grasping  articulators.  The  file  is  divided  into  six  major  sections: 

1 .  General  functions  and  variables 

This  section  contains  functions  and  variables  used  by  all  the  other 
sections  in  the  file.  Many  of  the  variables  will  come  from  the  sensing 
on  the  hand.  For  exan^le  *contact-points*,  from  the  fingertip  and 
joint  sensors.  Other  variables  must  be  specified,  but  may  in  the 
in  the  future  be  sensed  as  well.  For  instance,  *cg*  the  center  of 
gravity  of  the  object,  and  ‘contact-surfaces*  the  local  surface  shape 
in  the  area  of  the  contact . 

1 . 1  Define  general  global  variables 

1.2  Set  initial  values  of  the  general  global  variables 

1.3  General  utility  functions 

1.3.1  General  math  functions 

1.3.2  General  matrix  manipulation  functions 

1.3. 2.1  Major  manipulation  functions 

a.  ADD-ARRAY-LIST 

b.  MULTIPLY-ARRAY-LIST 

C .  ROW-REDUCED-ECHELON-MATRIX 

d.  TRANSFORM-DIRECTION-GLOBAL-FRAME 

e .  TRANSFORM-DIRECTION-FRAME -GLOBAL 

f .  TRANSFORM-POINTS-GLOBAL-FRAME 

g.  TRANSFORM-POINTS-FRAME-GLOBAL 

h.  TRANS FORM-FRAMES -GLOBAL-FRAME 

i.  TRANSFORM-FRAMES-FRAME-GLOBAL 

j.  GENERATE -FRAME 

k.  GENERATE-FRWffi-POINTS 

1.3. 2. 2  Auxiliary  functions 

a.  ADD-ARRAY-LIST 

b.  MULTIPLY-ARRAY-LIST 

C .  ROW-REDUCED-ECHELON-MATRIX 

d.  TRANSFORM-DIRECTION-GLOBAL-FRAME 

e .  TRANSFORM-DIRECTION-FRAME-GLOBAL 

f.  TRANSFORM-POINTS-GLOBAL-FRAME 

g.  TRANSFORM-POINTS-FRAME-GLOBAL 

h.  TRANSFORM-FRAMES -GLOBAL-FRAME 

i.  TRANSFORM-FRAMES-FRAME-GLOBAL 

j.  GENERATE-FRAME 

k.  GENERATE-FRAME-POINTS 

l.  General 

1.3.3  General  matrix  output  functions 

1.3. 3.1  Major  matrix  output  functions 
a.  PRINT-ARRAY-LIST 

1.3. 3. 2  Auxiliary  functions 

a.  PRINT -ARRAY-LIST 

b.  General 

1 . 4  Graphics  functions 

1.4.1  Define  graphics  variables 

1.4.2  Defind  functions  to  set  graphics  variables 

1.4.3  Set  graphics  functions 

1.4.4  Graphics  functions 

1.4. 4.1  Screen  creation  and  initialization  functions 
a.  MAKE-GRASP -SCREEN 


e : 


b.  KILL-GRASP-SCREEN 
C.  START-MONITOR-MSG 

d.  CREATE-GRASP-SCREEN 

e.  CLEARSCREEN 

1 . 4 . 4 . 2  Drawing  functions 

a.  DRAW-2D-GRASP-WINDOW 

b.  DRAW-3D-GRASP-WINDOW 

c.  SPHERE-3D-GRASP-WINDOW 

d.  DRAW-COORDINATE-SYSTEM 

e.  DRAW-COORDINATES 

f.  DRAW-3D-LIST 

1.4.5  Auxiliary  graphics  functions 

2 .  Sensed  and  global  variables 

This  section  defines  and  initializes  sensed  and  global  variables  used  in 
the  other  sections .  The  only  variables  which  are  truely  sensed  externally  ar 


d 

3 

t 

bles 


of 

! 
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♦contact -points*  a  list  of  contact -points  in  the  hand  space 

♦contact -normals*  a  list  of  normals  at  the  contact  points  in  han 

space . 

The  force  and  tactile  sensing  fingertip  need  to  be  working  for  these  variable 

to  be  read.  Until  then  these  variables  will  have  to  be  constructed.  So  there 
are  a  number  of  functions  and  variables  which  are  used  to  construct  the  varia 

♦contact-points*  and  *contact-normals* .  These  functions  and  variables  are 
temporary  and  are  used  only  for  simulation.  When  the  sensors  are  connected 
these  variables  and  functions  will  have  to  be  removed. 

Temporary  variables  used  to  construct  ‘contact -points* 

♦contact-points-object-space*  a  list  of  contact  points  in  the 

object  space 

*contact-normals-ob ject-space*  a  list  of  contact  normals  in  the 

object  space 

♦eg*  the  center  of  gravity  of  the  object 

♦major-axis*  the  normal  indicating  the  direction 

major  axis  of  an  axisymmetric  objec 


3.  Constraint  functions 

This  section  contain  functions  which  analysis  the  constraint  created  by 
the  contacts  on  the  grasped  object.  An  infinitesimal  analysis  involving 
virtual  work  and  a  finite  motion  analysis  involving  the  shape  of  contact 
surface  are  both  included  in  determine  the  overall  constraint  imposed  by 
the  contacts 

3.1  Define  constraint  variables 

3.2  Set  constraint  variables 

3.2.1  Functions  for  setting  constraint  variables 

3.3  Constraint  analysis  functions 

3.3.1  Major  analytic  functions 

a .  CONSTRUNT-VIRTUAL-WORK-LIST 

b.  DETERMINE -CONTACT-TYPES 

3.3.1  Auxiliary  functions 


4 .  Body  wrench 


g 

set 


In  this  section  the  wrench  on  the  object  resulting  from  forces  excluding 
contact  forces  from  the  robot  is  calculated.  Body  wrenches  may  result  from 
gravity,  accelerations,  electromagnetic  forces,  and  contacts  other  than 
fingertips.  The  only  body  wrenches  calculated  in  the  present  program  are 
those  resulting  from  gravity. 

4.1  Define  body  wrench  variables 

4.2  Set  body  wrench  variables 

4.3  Body  wrench  functions 

4.3.1  Major  body  wrench  functions 
a.  ORIENTATION 

4.3.2  Auxiliary  functions 
a.  ORIENTATION 

5 .  Contact  wrenches 

The  wrenches  in  the  contact  space  for  each  contact  are  calculated  assumin 
a  certain  stiffness  at  the  fingertips,  a  body  wrench  (section  3),  and  an  off 
wrench  (section  6) . 

5.1  Define  contact  wrench  variables 

5.2  Set  contact  wrench  variables 

5.2.1  Functions  to  set  contact  wrench  variables 

5 . 3  Contact  wrench  functions 

5.3.1  Major  contact  wrench  functions 
a .  CONSTUCT-CONTACT-WRENCH 

5.3.2  Auxiliary  functions 

a .  CONSTUCT-CONTACT-WRENCH 


al 

ts 


6 .  Contact  types 

In  this  section  the  type  of  contact  is  determined  given  certain  friction 
criteria  and  the  wrench  in  the  contact  space.  The  different  types  of  contac 
are 

(1)  Soft  finger  contact 

(2)  Point  contact  with  friction 

(3)  Point  contact  without  friction 

(4)  No  contact 

6.1  Define  contact  type  variables 

6.2  Set  contact  type  variables 

6 . 3  Contact  type  functions 

6.3.1  Major  contact  type  functions 
a.  CONTACT-TYPE 

6.3.2  Auxiliary  functions 
a.  CONTACT -TYPE 

7.  Offset  wrench 

The  internal  grasping  force  exerted  by  the  contacts  on  an  object  are  not 
determined  by  external  forces  and  may  be  varied  arbitarily  on  an  object. 

The  space  of  possible  solutions  to  the  grasping  force  problem  varies  with 
the  number  of  contacts: 


Number  of  contacts 


Dimension  of  solution  space 


1  0 

2  1 

3  3 

4  6 

n  3*(n-2)  n>-3 

For  a  two  fingered  grasp,  the  dimension  of  the  solution  space  on  the 

internal  grasping  forces  is  one.  That  is,  in  general,  the  squeeze  force 

between  the  two  fingers  may  be  varied. 

For  three  fingers  there  is  a  three  dimensional  space  of  solutions. 
Conceptually  this  space  can  be  simplified.  If  there  are  three  forces 
exerted  on  an  object,  and  at  least  one  force  is  not  parallel  to  the  others, 
and  the  object  is  not  accelerating,  then  the  three 

forces  intersect  at  a  point  in  space.  Furthermore,  these  intersection  point 

lie  on  the  plane  formed  by  the  three  contacts  points.  The  three  dimensional 
solution  space  of  grasping  forces  is  then  a  point  (X,Y)  lying  on  the  graspin 

force  plane,  and  F  the  grasping  force  magnitude. 

For  more  than  three  fingers,  the  solution  space  grows  by  3*(n-2),  where  n 
is  the  number  of  contacts. 

7 . 1  CONSTRUCT-OFFSET-WRENCH 

7.2  OFFSET-WRENCH-THREE-CONTACTS 


8 .  Controlled  slip 

This  is  an  experimental  section  formed  of  functions  which  use  the  functio 
of  the  previously  descibed  sections.  The  number  of  controllable  variables  t 
execute  dexterous  control  of  an  object  within  a  grasp  of  a  three  fingered  ha 
is  enormous: 

Controllable  variables  Dimension  of  space 


Grasping  force 

Orientation 

Stiffness 


3 

3 

9 


(more  here) 

8.1  Define  controlled  slip  variables 

8.2  Set  controlled  slip  variables 

8.3  Controlled  slip  variables 

8.3.1  PERMISSIBLE  TWIST 

accepts:  twist 
assumes:  orientation 

grasping  force  center 

returns:  maximally  constrained  state  which  allows  the 
allows  the  specified  twist 
orientation 

grasping  force  magnitude 

8.3.2  DETERMINE-CONSTRAINT-STATE  accepts:  grasping  force  center 

grasp  force  magnitude 
assumes:  orientation 
returns:  constraint  state 


8.3.3  MAP -CONSTRAINT-SPACE 


accepts :  force-magnitude 
assumes :  orientation 
stiffness 

returns:  two-dimensional  map  of  constrain  states 

as  a  function  of  (x,y)  the  grasping  force 
center 


;  9 .  Hand  actuation  functions 

;  This  section  contains  functions  to  perform  actual  motions  of  the  hand. 

;  9.1  Define  actuations  variables 

;  9.2  Define  functions  to  set  actuation  functions 

;  9.3  Set  actuation  variables 

;  9.4  Grasp  functions 

;  9.4.1  MOVE-TO-CONTACT-POINTS  assumes:  *contact-frames* 

;  returns;  moves  fingers  to  points  defined 

in  *contact-f raraes* 

;  9.4.2  GRASP  accepts:  grasping  force  center 

;  grasp  force  magnitude 

;  assumes:  stiffness 

;  returns:  motion  of  the  fingers 

;  9.5  Grasp  auxiliary  functions 

;  9.5.1  MOVE-TO-CONTACT-POINTS  auxiliary  functions 

;  9.5.2  GRASP  auxiliary  funcitons 

;  10.  Menu 

;  Standard  motion  options: 


Screen  graphics  options: 


CLEARSCREEN 


:  clears  "grasp-window"  pane  of  "grasp-scr 


ystems 


DRAW  COORDINATE  SYSTEM 


MAP  CONSTRAINT  SPACE 


screen 

: draws  the  three  dimensional  coordinate  s 

for  the  hand,  object,  and  contacts 
:plots  the  constraint  states  on  the  two 
dimensional  grasp  surface  for  a  specifie 


Controlled  slip  options: 


RESET  GLOBAL  VARIABLES 


grasp  force  magnitude 


:  shows  the  current  values  of  the  global 
variables  and  allows  the  user  to  change 


DETERMINE  CONSTRAINT  STATE: accepts  the  grasping  force  center  and  th 


PERMISSIBLE  TWIST 


grasping  force  magnitude 
assumes  the  stiffness  and  orientation 
and  returns  the  constraint  state 
: accepts  a  twist 
assumes  an  orientation 

and  returns  the  maximally  constrained  st 


Hand  actuations  options: 


which  allows  that  twist 


ng  to 
/ 

$ 

1 

ose 

joy 

t 

3  the 


MOVE  TO  CONTACT  POINTS  :moves  fingers  to  the  points  cooresspondi ! 

those  in  ^contact-frames* 

PICK  GRASP  FORCE  CENTER  :plots  the  contacts  on  the  two  dimensional 

grasp  surface  and  allows  the  user  to  cho! 
a  particular  force  magnitude  and  use  the ! 
stick  to  pick  a  force  center  and  actuate ! 
hand  accordingly 


n 


Advanced  hand  actuation  options 

CONTROLLED  SLIP  rallows  the  user  to  enter  an  object  motio! 

and  the  hand  tries  to  actuate  it . 


he 


11.  Demos 

This  section  contains  demo  programs  which  are  based  on  the  analyses  of  t! 
the  previous  section. 


.  ..***«******ili**********«**«***«*i>****«****'*******ilt«*****ilr*ilrit*ilt«ilr***************'********l 

*  ★  ★  ★  ★ 

;  1 .  General  functions  and  variables 

*  *  *  *  * 

;  1.1  Define  variables 

;  1.2  Set  default  variable  values  section 

;  1.2.1  Functions  to  set  default  variable  values 

;  1.2.2  Set  default  variable  values 

;  1.3  General  utility  functions 

;  1.3.1  General  math  functions 

;  1.3.1. 1  SQR 

(defun  sqr  (x) 

(cond  ( (not  (numberp  x) )  0) 

(t  (*  X  X) ) )  ) 

;  1.3.2  General  matrix  manipulation  functions 

;  1.3. 2.1  ADD-ARRAY-LIST 

(defun  add-array-list  (array-list) 

(cond  ( (null  array-list)  nil) 

( (atom  array-list)  array-list) 

(t  (add-two-arrays  (add-array-list  (car  array-list) ) 

(add-array-list  (cdr  array-list) ) ) ) ) ) 

;  1.3. 2.1. A  ADD-ARRAY-LIST  Auxiliary 

(defun  add-two-arrays  (array-1  array-2) 

(cond  ((and  (not  array-1)  (not  array-2))  nil) 

( (not  array-1)  array-2) 

{ (not  array-2)  array-1) 

( (vectorp  array-1) 

(let*  ((elements  (array-dimension  array-1  0)) 

(sum-array  (make-array  elements) ) ) 

(do  (  (i  0  (+  i  1)  ).) 


( (-  i  elements)  sum-array) 

(setf  (aref  sum-array  i)  (+  (aref  array-1  i)  (aref  array-2  i) ) ) ) ) ) 

(t  (let*  ((rows  (array-dimension  array-1  0)) 

(columns  (array-dimension  array-2  1) ) 

(sum-array  (ma)ce-array  (list  rows  columns) ) ) ) 

(do  ((  i  0  (+  i  1))) 

( (-  i  rows)  sum-array) 

(do  ( ( j  0  (+  j  1) ) ) 

( (-  j  columns)  nil) 

(setf  (aref  sum-array  i  i)  (+  (aref  array-1  i  j)  (aref  array-2  i  j))))) 

) ) )  ) 


;  1.3. 2. 2  MULTIPLY-ARRAY-LIST 

(defun  multiply-array-list  (array-list) 

(cond  ( (null  array-list)  nil) 

( (atom  array-list)  array-list) 

(t  (multiply-two-arrays  (multiply-array-list  (car  array-list) ) 
(multiply-array-list  (cdr  array-list) ) ) ) ) ) 

;  1.3. 2. 2. A  MULTIPLY-ARRAY-LIST  Auxiliary 

(defun  multiply-two-vectors  (array-1  array-2) 

(let*  ( (elements-l  (array-dimension  array-1  0)) 

(elements-2  (array-dimension  array-2  0)) 

(product-array  (initialize-array  elements-l  element3-2) ) ) 

(cond  ((not  (-  elements-l  elements-2))  nil) 

(t 

(do  (  ( j  0  (+  j  1) ) ) 

( (»  j  elements-l)  product-array) 

(do  ((k  0  (+  k  1))) 

( (-  k  elements-l)  nil) 

(setf  (aref  product-array  j  k) 

(*  (aref  array-1  j)  (aref  array-2  k) ))))))) ) 

(defun  multiply-array-vector  (array  vector) 

(let*  ( (rows-1  (array-dimension  array  0)) 

(elements  (array-dimension  vector  0)) 

(product-array  (initialize-vector  elements))) 

(do  ((j  0  (+  j  1))) 

( (-  j  rows-1)  product-array) 

(do  ((k  0  (+  k  1))) 

( (-  k  elements)  nil) 

(setf  (aref  product-array  j) 

(+  (aref  product-array  j) 

(*  (aref  array  j  k)  (aref  vector  k) )))))) ) 

(defun  multiply-two-arrays  (array-1  array-2) 

(cond  ((and  (not  array-1)  (not  array-2))  nil) 

( (not  array-1)  array-2) 

( (not  array-2)  array-1) 

((and  (not  (arrayp  array-1))  (not  (arrayp  array-2)))  (*  array-1  array-2)) 
((not  (arrayp  array-1))  (multiply-constant-array  array-1  array-2)) 

((not  (arrayp  array-2))  (multiply-constant-array  array-2  array-1)) 

( (vectorp  array-1) 

(cond  ((not  (-  (array-rank  array-2)  1) )  nil) 

(t  (multiply-two-vectors  array-1  array-2)))) 

( (not  (equalp  (array-dimension  array-1  1) 

(array-dimension  array-2  0) ) )  nil) 

( (vectorp  array-2)  (multiply-array-vector  array-1  array-2) ) 

(t  (let*  ((rows-1  (array-dimension  array-1  0)) 

(rows-2  (array-dimension  array-2  0) ) 

(columns-2  (array-dimension  array-2  1) ) 

(product-array  (initialize-array  rows-1  columns-2) ) ) 

(do  ((  i  0  (+  i  1))) 

( (••  i  columns-2)  product-array) 

(do  ( ( j  0  (+  j  1) )  ) 

( (“  j  rows-1)  nil)p 


(do  ((k  0  (+  k  1))) 

( (-  k  rows-2)  nil) 

(setf  (aref  product-array  j  i) 

(+  (aref  product-array  j  i) 

(*  (aref  array-1  j  k)  (aref  array-2  k  i) ))))))))) ) 


(defun  multiply-constant-array  (constant  array) 

(cond  ( (vectorp  array) 

(let*  ((elements  (array-dimension  array  0)) 

(product -array  (make-array  elements) ) ) 

(do  ( (i  0  (+  i  1) ) ) 

( (-  i  elements)  product-array) 

(setf  (aref  product-array  i)  (*  constant  (aref  array  i) ) ) ) ) ) 

(t 

(let*  ((rows  (array-dimension  array  0)) 

(columns  (array-dimension  array  1) ) 

(product-array  (make-array  (list  rows  columns) ) ) ) 

(do  ((i  0  (+  i  1))) 

( (»  i  rows)  product-array) 

(do  ((j  0  (+  j  1))) 

( (-  j  columns)  nil) 

(setf  (aref  product-array  i  j)  (*  constant  (aref  array  i  j) )))))))) 

;  1 . 3 . 2 . 3  ROW-REDUCED -ECHELON 

(defun  row- reduced-echelon  (array) 

(let  ((rows  (array-dimension  array  0)) 

(colui'iis  (array-dimension  array  1))) 

(row-reduced-echelon-aux  (diagonalize  array)  (  -  rows  1)  0  rows  columns) ) ) 


1.3. 2. 3. A  ROW-REDUCED-ECHELON  Auxiliary 


(defun  zero-array  (rows  columns) 

(cond  ((or  (<  rows  0)  (<  coltimns  0))  nil) 

((and  (-  rows  0)  (-  columns  0))  nil) 

( (-  rows  0)  (zero-vector  columns)) 

( (■  columns  0)  (zero-vector  rows)) 

(t 

(let  ((array  (make-array  (list  rows  columns)))) 
(do  ((i  0  (+  i  1))) 

( (■  i  rows)  array  ) 

(do  ((j  0  (+  j  1))) 

( (-  j  columns) ) 

(setf  (aref  array  i  j)  0))))))) 


(defun  zero-vector  (elements) 

(cond  ( (<“  elements  0)  nil) 

(t 

(let  ((vector  (make-array  elements))) 
(do  ((i  0  (+  i  1))) 

( (»  i  elements)  vector) 

(setf  (aref  vector  i)  0)))))) 


(defun  row-reduced-echelon-aux  (array  current-row  current -column  rows  columns) 

(cond  ( (“  current-row  0)  array) 

( (zero-row  array  current -column  current-row) 

(row-reduced-echelon-aux  array  (-  current-row  1)  0  rows  columns)) 

( (-  current-column  columns) 

(row-reduced-echelon-aux  array  (-  current-row  1)  0  rows  columns) ) 

( (<  (sqr  (aref  array  current-row  current-column))  0.00001) 
(row-reduced-echelon-aux  array  current-row  (+  1  current-column)  rows  columns) ) 

(t  (row-reduced-echelon-aux 

(row-reduced-echelon-aux-1  array  current-row  current-row  current-column  row! 


(-  current-row  1) 
0  rows  columns) ) ) ) 


(defun  row-reduced-echelon-aux-1  (array  inc-row  current-row  current-column  rows) 


(cond  ( (-  inc-row  0)  array) 

(t 

(row-reduced-echelon-aux-1 

(add-x-time3-rl-row-to-r2-row  array  current-row  current -column  (-  inc-row  1) ) 
(-  inc-row  1)  current-row  current-column  rows) ) ) ) 

(defun  diagonalize  (array) 

(let  ((rows  (array-dimension  array  0)) 

(columns  (array-dimension  array  1))) 

(diagonalize-array-aux  array  0  0  rows  columns) ) ) 

(defun  zero-column  (array  current -column  current-row) 

(let  ((rows  (array-dimension  array  0)) 

(sum  0) ) 

(do  ( (i  current-row  (+  i  1) )  ) 

( (-  i  rows)  (<  (sqr  sum)  0.00001)) 

(setq  sum  (+  sum  (abs  (aref  array  i  current-column))))))) 

(defun  zero-row  (array  current-column  current-row) 

(let  ((columns  (array-dimension  array  1)) 

(sum  0) ) 

(do  ( (i  current-column  (+  i  1) ) ) 

( (-  i  columns)  (<  (sqr  sum)  0.00001)) 

(setq  sum  (+  sum  (abs  (aref  array  current-row  i) ) ) ) )  ) ) 

(defun  diagonalize-array-aux  (array  current-column  current-row  rows  columns) 

(cond  ( (-  current-column  (min  rows  columns) )  array) 

( (zero-column  array  current-coltxnn  current-row) 

(diagonalize-array-aux  array  (+  1  current-column)  current-row  rows  columns)) 

(t 

(diagonalize-array-aux 

(initialize-column-aux 

(divide-r-row-by-element-rc  array  current-row  current-column) 
current -column  current-row  (+  1  current-row)  rows) 

(+  1  current-column) 

(+  1  current-row) 
rows 

columns) ) ) ) 

(defun  initialize-column-aux  (array  initial-column  initial-row  current-row  rows) 

(cond  ( (-  current-row  rows)  array) 

(t 

( initialize-column-aux 

(add-x-time3-rl-row-to-r2-row  array 

initial-row  initial-column  current-row) 
initial-column 
initial-row 
(+  1  current-row) 
rows) ) ) ) 

(defun  divide-r-row-by-element-rc  (array  r  c) 

(let  ((columns  (array-dimension  array  1)) 

(first-element  (aref  array  r  c) ) 

(new-array  (initialize  array) ) ) 

(cond  ( (<  (sqr  first-element)  0.00001) 

(divide-r-row-by-element-rc  (interchange-last-row-and-row-r  array  r)  r  c) ) 

(t 

(do  ( (i  c  (+  i  1) ) ) 

( (“  i  columns)  new-array) 

(setf  (aref  new-array  r  i)  (/  (aref  array  r  i)  first-element))))))) 


(defun  add-x-times-rl-row-to-r2-row  (array  rl  cl  r2) 
(cond  ( (<  (sqr  (aref  array  rl  cl))  0.00001)  array) 
(t 

(let  ((columns  (array-dimension  array  1)) 


(new-array  (initialize  array)) 

(constant  (*  -1  (aref  array  r2  cl)))) 

(do  ((id  (+  i  1) ) ) 

( (»  i  columns)  new-array) 

(setf  (aref  new-array  r2  i)  (+  (*  constant  (aref  array  rl  i)  ) 

(aref  array  r2  i)) )))))) 

(defun  interchange-last-row-and-row-r  (array  r) 

(let  ((last-row  (-  (array-dimension  array  0)  1)) 

(columns  (array-dimension  array  1) ) 

(new-array  (initialize  array))) 

(do  ( (i  0  (+  i  1) ) ) 

( (“  i  columns)  new-array) 

(setf  (aref  new-array  r  i)  (aref  array  last-row  i) ) 

(setf  (aref  new-array  last-row  i)  (aref  array  r  i) ) ) ) ) 

(defun  transpose-contact-frame  (contact-frame) 

(let*  ( (t-contact-frame  (initialize  contact-frame))) 

(do  (  (i  0  (+  i  1)  ) ) 

( (“  i  3)  t-contact-frame) 

(do  {  (j  0  (+  j  1) ) ) 

(  (-  j  3)  ) 

(setf  (aref  t-contact-frame  i  j)  (aref  contact-frame  j  i) ) ) ) ) ) 

;  1.3. 2. 4  TRANSFORM-DIRECTION-GLOBAL-FRAME 

(defun  transform-direction-global-frame  (frames  objects) 

(cond  ((and  (listp  objects)  (listp  frames)) 

(transform-direction-global-frame-lists  frames  objects) ) 

( (listp  objects) 

(transform-direction-global-frame-list  frames  objects) ) 

(t 

(transform-direction-ob ject-global-frame  frames  objects) ) ) ) 

;  1.3. 2. 4. A  TRANSFORM-DIRECTION-GLOBAL-FRAME  Auxi 

liary 

(defun  transform-direction-global-frame-lists  (frames  objects) 

(cond  ((null  frames)  nil) 

(t  (cons  (transform-direction-object-global-frame  (car  frames)  (car  objects) ) 

(transform-direction-global-frame-lists  (cdr  frames)  (cdr  objects)))))) 

(defun  transform-direction-global-frame-list  (frame  objects) 

(cond  ( (null  objects)  nil) 

(t  (cons  (transform-direction-ob ject-global-frame  frame  (car  objects) ) 

(transform-direction-global-frame-list  frame  (cdr  objects) ) ) ) ) ) 

(defun  transform-direction-ob ject-global-frame  (frame  object) 

(cond  ( (vectorp  object)  (transform-direction-vector-global-frame  frame  object) ) 

( (arrayp  object)  (transform-direction-array-global-frame  frame  object) ) 

(t  nil)  )■ ) 

(defun  transf orm-dirertion-array-global-f rame  (frame  array) 

(let*  ((rows  (array-dimension  array  0)) 

(columns  (array-dimension  array  1) ) 

(new-array  (ma)ce-array  (list  rows  columns)))) 

(do  (  (i  0  (+  i  3)  )  ) 

( (”  i  rows)  new-array) 

(do  (  ( j  0  (+  j  1)  )  ) 

( (•  j  columns) ) 

(do  (  ()c  0  (-f  k  1)  )  ) 

(  (=  k  3)  ) 

(setf  (aref  new-array  (+  i  k)  j) 

(+  (*  (aref  array  i  j)  (aref  frame  0  k) ) 

(*  (aref  array  (+  i  1)  j)  (aref  frame  1  k) ) 

(*  (aref  array  (+  i  2)  j)  (aref  frame  2  k) ))))))) ) 

(defun  transform-direction-vector-global-frame  (frame  vector) 

(let*  ((elements  (array-dimension  vector  0)) 


(new-vector  (make-array  elements) ) ) 

(do  ( (i  0  (+13))) 

( (•  i  elements)  new-vector) 

(do  ((k  0  (+  k  1) ) ) 

(  (-  k  3)  ) 

(setf  (aref  new-vector  (+  i  k) ) 

(+  (*  (aref  vector  i)  (aref  fr2une  0  k) ) 

(*  (aref  vector  (+  i  1))  (aref  frame  1  k) ) 

(*  (aref  vector  (+  i  2))  (aref  frame  2  k) )))))) ) 

;  1.3. 2. 5  TRANSFORM-DIRECTION-FRAME-GLOBAL 

(defun  transform-direction-frame-global  (frames  objects) 

■ (cond  ((and  (listp  objects)  (listp  frames)) 

(transform-direction-frame-global-lists  frames  objects) ) 

((listp  objects) 

(transform-direction-frame-global-list  frames  objects) ) 

(t 

(transform-direction-object-frame-global  frames  objects) ) ) ) 

;  1.3. 2. 5. A  TRANSFORM-DIRECT ION-FRAME -GLOBAL  Auxi 

liary 

(defun  transform-direction-frame-global-lists  (frames  objects) 

(cond  ( (null  frames)  nil) 

(t  (cons  (transform-direction-object-frame-global  (car  frames)  (car  objects)) 

(transform-direction-frame-global-lists  (cdr  frames)  (cdr  objects) ) ) ) ) ) 

(defun  transform-direction-frame-global-list  (frame  objects) 

(cond  ( (null  objects)  nil) 

(t  (cons  (transform-direction-object-frame-global  frame  (car  objects) ) 

(transform-direction-frame-global-list  frame  (cdr  objects)))))) 

(defun  transform-direction-object-frame-global  (frame  object) 

(cond  ( (vectorp  object)  (transform-direction-vector-frame-global  frame  object) ) 

( (arrayp  object)  (transform-direction-array-frame-global  frame  object) ) 

(t  nil) ) ) 

(defun  transform-direction-array-trame-global  (frame  array) 

(let*  ((rows  (array-dimension  array  0)) 

(columns  (array-dimension  array  1)) 

(new-array  (make-array  (list  rows  columns) ) ) ) 

(do  ( (i  0  (+  i  3) )  ) 

( (=  i  rows)  new-array) 

(do  (  ( j  0  (+  j  1)  )  ) 

( (=  j  columns) ) 

(do  (  (k  0  {+  k  1) )  ) 

((-  k  3)) 

(setf  (aref  new-array  (+  i  k)  j) 

(+  (*  (aref  array  i  j)  (aref  frame  k  0)) 

(*  (aref  array  (+  i  1)  j)  (aref  fra.me  k  1)  ) 

(*  (aref  array  (+  i  2)  j)  (aref  frame  k  2))))))))) 

(defun  transform-direction-vector-frame-global  (frame  vector) 

(let*  ((elements  (array-dimension  vector  0)) 

(new-vector  (make-array  elements) ) ) 

(do  (  (i  0  (+  i  3)  )  ) 

(  (”  i  elements)  i.ew-vector) 

(do  (  (k  0  (+  k  1)  )  ) 

(  (»  k  3)  ) 

(se.f  (aref  new-vector  (+  i  k) ) 

(+  (*  (aref  vector  i)  (aref  frame  k  0)) 

(*  (aref  vector  (+  i  1))  (aref  frame  k  1)) 

(*  (aref  vector  (+  i  2) )  (aref  frame  k  2)))))))) 

;  1.3. 2. 6  TRANSFORM-POINTS -GLOBAL-FRAME 

(defun  transf orm-points-global-f rame  (frames  objects) 

(cond  ((and  (listp  objects)  (listp  frames)) 


(transform-points-global-frame-liats  frames  objects) ) 

( (listp  objects)  * 

(transf orm-points-global-frame-list  frames  objects) ) 

(t 

(transf orm-points-object-global-frame  frames  objects)))) 

;  1.3. 2. 6. A  TRANSFOBMP-POINTS-FRAME-GLOBAL  Auxili 

ary 

(defun  transf orm-points-global-frame-lists  (frames  objects) 

(cond  ( (null  frames)  nil) 

(t  (cons  (transform-points-object -global-frame  (car  frames)  (car  objects)) 

(transform-points-global-frame-lists  (cdr  frames)  (cdr  objects)))))) 

(defun  transform-points-global-f rame-list  (frame  objects) 

(cond  ( (null  objects)  nil) 

(t  (cons  (transform-points-object -global-frame  frame  (car  objects) ) 

(transform-points-global-frame-list  frame  (cdr  objects)))))) 

(defun  transform-points-ob ject-global-f rame  (frame  object) 

(cond  ( (vectorp  object)  (transform-points-vector-global-frame  frame  object) ) 

( (arrayp  object)  (transf orm-points-array-global-f rame  frame  object)) 

(t  nil) ) ) 

(defun  transf orm-points-array-global-f rame  (frame  array) 

(let*  ((rows  (array-dimension  array  0)) 

(columns  (array-dimension  array  1)) 

(new-array  (make-array  (list  rows  columns) ) ) ) 

(do  (  (i  0  (+  i  3)  )  ) 

( (=  i  rows)  new-array) 

(do  (  ( j  0  (+  j  1) ) ) 

( (=  j  columns) ) 

(do  (  (k  0  (+  k  1) )  ) 

(  (-  k  3)  ) 

(setf  (aref  new-array  (+  i  k)  j) 

(+  (*  (-  (aref  array  i  j)  (aref  frame  0  3))  (aref  frame  0  k) ) 

(*  (-  (aref  array  (+  i  1)  j)  (aref  frame  1  3))  (aref  frame  1  k) ) 

(*  (-  (aref  array  (+  i  2)  j)  (aref  frame  2  3))  (aref  frame  2  k) ) ) > ) ) ) 

)  ) 

(defun  transf orm-points-vectcr-global-f rame  (frame  vector) 

(let*  ( (elements  (array-dimension  vector  0) ) 

(new-vector  (make-array  elements) ) ) 

(do  (  (i  0  (+  i  3)  )  ) 

( (=  i  elements)  new-vector) 

(do  (  (k  0  (+  k  1)  )  ) 

(  (-  k  3)  ) 

(setf  (aref  new-vector  (+  i  k) ) 

(+  (*  (-  (aref  vector  i)  (aref  frame  0  3))  (aref  frame  0  k) ) 

(*  (-  (aref  vector  (+  i  1))  (aref  frame  1  3))  (aref  frame  1  k) ) 

(*  (-  (aref  vector  (+  i  2))  (aref  frame  2  3))  (aref  frame  2  k) )))))) ) 

;  1.3. 2. 7  TRANSFORM-POINTS -FRAME-GLOBAL 

(defun  transf orm-points-frame-global  (frames  objects) 

(cond  ((and  (listp  objects)  (listp  frames)) 

(transform-points-frame-global-lists  frames  objects) ) 

( (listp  objects) 

(transform-points-f rame-global-list  frames  objects) ) 

(t 

(transf orm-points-object-frame-global  frames  objects) ) ) ) 

;  1.3. 2. 7. A  TRANSFORM-POINTS-FRAME-GLOBAL  Auxilia 

ry 

(defun  transform-points-frame-global-lists  (frames  objects) 

(cond  ( (null  frames)  nil) 

(t  (cons  (transform-points-ob ject-frame-global  (car  frames)  (car  objects) ) 

(transf orm-points-frame-global-lists  (cdr  frames)  (cdr  objects)))))) 


(defun  transform-points-f rame-global-list  (frame  objects) 

(cond  ((null  objects)  nil) 

(t  (cons  (transform-points-object-f rame-global  frame  (car  objects) ) 

(transform-points-f rame-global-list  frame  (cdr  objects) ) ) ) ) ) 

(defun  transform-points-object-f rame-global  (frame  object) 

(cond  ( (vectorp  object)  (transform-points-vector-freune-global  frame  object)) 

( (arrayp  object)  (transform-points-array-f rame-global  frame  object)) 

(t  nil) ) ) 

(defun  transform-points-array-f rame-global  (frame  array) 

(let*  ((rows  (array-dimension  array  0)) 

(columns  (array-dimension  array  1) ) 

(new-array  (ma)te-array  (list  rows  columns) ) ) ) 

(do  ( (i  0  (+  i  3)  )  ) 

( (“  i  rows)  new-array) 

(do  (  ( j  0  (+  j  1)  )  ) 

( (-  j  columns) ) 

(do  (  (k  0  (-i-  k  1)  )  ) 

(  (-  k  3)  ) 

(setf  (aref  new-array  (+  i  k)  j) 

(+  (*  (aref  array  i  j)  (aref  frame  k  0)) 

(*  (aref  array  (+  i  1)  j)  (aref  frame  k  1) ) 

(*  (aref  array  (+  i  2)  j)  (aref  frame  k  2) ) 

(aref  frame  k  3)))))))) 

(defun  transform-points-vector-f rame-global  (frame  vector) 

(let*  ( (elements  (array-dimension  vector  0) ) 

(new-vector  (make-array  elements))) 

(do  (  (i  0  (+  i  3)  )  ) 

( (”  i  elements)  new-vector) 

(do  (  (k  0  (+  k  1) )  ) 

((=  k  3)) 

(setf  (aref  new-vector  (+  i  k) ) 

(+  (*  (aref  vector  i)  (aref  frame  k  0)) 

(*  (aref  vector  (+  i  1))  (aref  frame  k  1)) 

(*  (aref  vector  (+  i  2))  (aref  frame  k  2)) 

(aref  frame  k  3) ) ) ) ) ) ) 

;  1.3. 2. 8  TRANSFORM-FRAMES-GLOBAL-FRAME 

(defun  transf orm-f rames-global-f rame  (frames  object-frames) 

(cond  ((and  (listp  object-frames)  (listp  frames)) 

(transform-f rames-global-f rame-lists  frames  object-frames) ) 

((listp  object-frames) 

(transform-f rames-global-f rame-list  frames  object-frames) ) 

(t 

(transform-frame-global-frame  frames  object-frames) ) ) ) 

;  1.3. 2. 8. A  TRANSFORM-FRAMES-GLOBAL-FRAME  Auxilia ! 

ry 

(defun  transf orm-f rames-global-f rame-lists  (frames  object-frames) 

(cond  ( (null  frames)  nil) 

(t  (cons  (transform-f rames-global-f rame  (car  frames)  (car  object-frames)) 

(transform-frames -global-frame-lists  (cdr  frames)  (cdr  object-frames) ) )  i 

)  )  ) 

(defun  transform-f rames-global-f rame-list  (frame  object-frames) 

(cond  ( (null  object-frames)  nil) 

(t  (cons  (transform-frame-global-frame  frame  (car  object-frames) ) 

(transform-frames-global-f rame-list  frame  (cd^-  object-frames)))))) 

(defun  transform-frame-global-frame  (frame  object-frame) 

(let*  ((new-object-frame  (make-array  '(3  4)))) 

(do  (  (i  0  (til))) 

(  (-  i  3)  ) 


(<-  j  3)) 

(setf  (aref  new-object -frame  j  i) 

(+  (*  (aref  object-frame  0  i)  (aref  frame  0  j) ) 

(*  (aref  object-frame  1  i)  (aref  frame  1  j)) 

(*  (aref  object-frame  2  i)  (aref  frame  2  j)))))) 

(do  ( (i  0  (+  i  1)  ) ) 

((-  i  3)  new-object -frame) 

(setf  (aref  new-object-frame  i  3) 

(+  (*  (-  (aref  object-frame  0  3)  (aref  frame  0  3))  (aref  frame  0  i) ) 

(*  (-  (aref  object-frame  1  3)  (aref  frame  1  3))  (aref  frame  1  i) ) 

(*  (-  (aref  object-frame  2  3)  (aref  frame  2  3))  (aref  frame  2  i) ) ) ) ) ) ) 

;  1.3. 2. 9  TRANSFORM-FRAMES-FRAME -GLOBAL 

(defun  transform-f rames-f rame-global  (frames  object-frames) 

(cond  ((and  (listp  object-frames)  (listp  frames)) 

(transform-f rames-f rame-global-lists  frames  object-frames) ) 

( (listp  object-frames) 

(transform-f rames-f rame-global-list  frames  object-frames) ) 

(t 

(transform-frame-frame-global  frames  object-frames) ) ) ) 

;  1.3. 2. 9. A  TRANSFORM-FRAMES-FRAME-GLOBAL  Auxilia 

ry 

(defun  transform-f rames-f rame-global-lists  (frames  object-frames) 

(cond  ( (null  frames)  nil) 

(t  (cons  (transform-frame-frame-global  (car  frames)  (car  object-frames) ) 

(transform-f rames-f rame-global-lists  (cdr  frames)  (cdr  object-frames)  )  ) 

) ) ) 

(defun  transform-f rames-f rame-global-list  (frame  object-frames) 

(cond  ( (null  object-frames)  nil) 

(t  (cons  (transform-frame-frame-global  frame  (car  object-frames) ) 

(transform-f rames-f rame-global-list  frame  (cdr  object-frames) ) ) ) ) ) 

(defun  transform-frame-frame-global  (frame  object-frame) 

(let*  ((new-object-frame  (ma)ce-array  '(3  4)))) 

(do  ( (i  0  (+  i  1) )  ) 

(  (-  i  3)  ) 

(do  ((j  0  (+  j  1))) 

((-  j  3)) 

(setf  (aref  new-object-frame  j  i) 

(+  (*  (aref  object-frame  0  i)  (aref  frame  j  0)) 

(*  (aref  object-frame  1  i)  (aref  frame  j  1)) 

(*  (aref  object-frame  2  i)  (aref  frame  j  2)))))) 

(do  (  (i  0  (+  i  1) ) ) 

( (=  i  3)  new-object-frame) 

(setf  (aref  new-object-frame  i  3) 

(+  (*  (aref  object-frame  0  3)  (aref  frame  i  0)) 

(*  (aref  object-frame  1  3)  (aref  frame  i  1)) 

(*  (aref  object-frame  2  3)  (aref  frame  i  2) ) 

(aref  frame  i  3) ) ) ) ) ) 

;  1.3.2.10  GENERATE -FRAME 

(defun  generate-f rame  (points  vectors) 

(cond  ( (null  points)  nil) 

( (atom  points)  (generate-f rame-aux  points  vectors) ) 

(t  (cons  (generate-f rame  (car  points)  (car  vectors) ) 

(generate-f rame  (cdr  points)  (cdr  vectors)))))) 

;  1.3.2.10  GENERATE -FRAME  Auxiliary 

(defun  generate-f rame-aux  (point  vector) 

(let*  ( (frame  (ma)<e-array  '  (3  4)  ) ) 

(length  (sqrt  (+  (sqr  (aref  vector  0)) 

(sqr  (aref  vector  1)) 

(sqr  (aref  vector  2))))) 


(nx  (/  (aref  vector  0)  length) ) 

(ny  (/  (aref  vector  1)  length)) 

(nz  (/  (aref  vector  2)  length)) 

(lx)  (ly)  (Iz)  (mx)  (my)  (mz) ) 

(cond  ( (-  nx  0)  (setq  lx  1)  (setq  ly  0)  (setq  Iz  0)) 

( (“  ny  0)  (setq  lx  0)  (setq  ly  -1)  (setq  Iz  0)) 

(t  (setq  lx  (sqrt  (/  (sqr  (/  ny  nx) )  (+  1  (sqr  (/  ny  nx) ) ) ) ) ) 

(setq  ly  (/  (*  -1  nx  lx)  ny) ) 

(setq  Iz  0) ) ) 

(setq  mx  (-  (*  ny  Iz)  (*  nz  ly) ) ) 

(setq  my  (-  (*  nz  lx)  (*  nx  Iz) ) ) 

(setq  mz  (-  (*  nx  ly)  (*  ny  lx))) 

(setf  (aref  frame  0  0)  lx) 

(setf  (aref  frame  0  1)  mx) 

(setf  (aref  frame  0  2)  nx) 

(setf  (aref  frame  0  3)  (aref  point  0)) 

(setf  (aref  frame  1  0)  ly) 

(setf  (aref  fraune  1  1)  my) 

(setf  (aref  frame  1  2)  ny) 

(setf  (aref  frame  1  3)  (aref  point  1)) 

(setf  (aref  fraune  2  0)  Iz) 

(setf  (aref  frame  2  1)  mz) 

(setf  (aref  frame  2  2)  nz) 

(setf  (aref  frame  2  3)  (aref  point  2)) 
frame)  ) 

;  1.3.2.11  GENERATE-FRAME-POINTS 

(defun  generate-f rame-points  (points-1  pointa-2) 

(cond  ( (null  pointa-1)  nil) 

((atom  points-l)  (generate-f rame-points-aux  points-1  points-2)) 

(t  (cons  (generate-f rame-points  (car  points-1)  (car  points-2) ) 

(generate-f rame-points  (cdr  points-1)  (cdr  points-2)))))) 

;  1.3.2.11  GENERATE-FRAME-POINTS  Auxiliary 

(defun  generate-f rame-points-aux  (point-1  point -2) 

(let*  ((length  (sqrt  (+  (sqr  (-  (aref  point-2  0)  (aref  point-1  0))) 

(sqr  (-  (aref  point-2  1)  (aref  point-1  1))) 

(sqr  (-  (aref  point-2  2)  (aref  point-1  2)))))) 

(nx  (/  (-  (aref  point-2  0)  (aref  point-1  0))  length)) 

(ny  (/  (-  (aref  point-2  1)  (aref  point-1  1))  length)) 

(nz  (/  (-  (aref  point-2  2)  (aref  point-1  2))  length)) 

(normal  (ma)ce-array  '  (3) ) )  ) 

(setf  (aref  normal  0)  nx) 

(setf  (aref  normal  1)  ny) 

(setf  (aref  normal  2)  nz) 

(generate-f rame-aux  point-1  normal) ) ) 

;  1.3.3  General-functions 

;  1 . 3 . 3 . 1  COUNT-ATOMS 

(defun  count-atoms  (list) 

(cond  ( (null  list)  0) 

( (atom  list)  1) 

( (+  (count-atoms  (car  list) ) 

(count-atoms  (cdr  list)))))) 

;  1.3. 3. 2  INITIALIZE-VECTOR 

(defun  initialize-vector  (elements) 

(let  ((vector  (ma)ce-array  elements))) 

(do  ((i  0  (+  i  1))) 

( (-  i  elements)  vector) 

(setf  (aref  vector  i)  0)))) 

;  1.3. 3. 3  INITIALIZE-ARRAY 

(defun  initial  ..ze-array  (rows  columns) 

(let  ((array  (make-array  (list  rows  columns)))) 

(do  ( (i  0  (+  i  1)  )  ) 


( (-  i  rows)  array) 

(do  ((j  0  (+  j  1))) 

( (“  j  columns)  nil) 
(setf  (aref  array  i  j) 


0) ) ) ) ) 


;  1.3. 3. 4  INITIALIZE 

(defun  initialize  (array) 

(let*  ((rows  (array-dimension  array  0)) 

(columns  (array-dimension  array  1) ) 

(new-array  (ma)ce-array  (list  rows  columns)))) 

(do  ((i  0  (+  i  1))) 

( (-  i  rows)  new-array  ) 

(do  ( (j  0  (+  j  1) ) ) 

( (“  j  columns) ) 

(setf  (aref  new-array  i  j)  (aref  array  i  j)))))) 

;  1 . 3 . 3 . 5  TRANSPOSE 

(defun  transpose  (array) 

(let*  ((rows  (array-dimension  array  0)) 

(columns  (array-dimension  array  1) ) 

(new-array  (make-array  (list  columns  rows) ) ) ) 

(do  ( (i  0  (+  i  1) ) ) 

( (=  i  rows)  new-array) 

(do  (  (j  0  (-t-  j  1) ) ) 

(  (»  j  columns) ) 

(setf  (aref  new-array  j  i)  (aref  array  i  j)))))) 

;  1 . 3 . 3 . 6  GET-ARRAY-COLUMN 

(defun  get-array-column  (array  column) 

(let*  ((rows  (array-dimension  array  0)) 

(vector  (make-array  rows) ) ) 

(do  ((i  0  (+  1  i))) 

( («  i  rows)  vector) 

(setf  (aref  vector  i)  (aref  array  i  column))))) 

;  1.3. 3. 7  ABS-ARRAY 

(defun  abs-array  (array-list) 

(cond  (  (null  array-list)  nil) 

( (atom  array-list)  (abs-one-array  array-list) ) 

(t  (cons  (abs-array  (car  array-list) ) 

(abs-array  (cdr  array-list) ) ) ) ) ) 

(defun  abs-one-array  (array) 

(cond  ( (vectorp  array)  (abs-one-vector  array) ) 

(t  (let*  ( (rows  (array-dimension  array  0) ) 

(columns  (array-dimension  array  1) ) 

(new-array  (make-array  (list  rows  columns) ) ) ) 

(do  ( (i  0  (+  i  1)  ) ) 

( («  i  rows)  new-array) 

(do  ( ( j  0  (+  j  1) )  ) 

( (-  j  columns) ) 

(setf  (aref  new-array  i  j)  (abs  (aref  array  i  j) )))))))) 


(defun  abs-one-vector  (vector) 

(let*  ((elements  (array-dimension  vector  0)) 

(new-vector  (make-array  elements) ) ) 

(do  ((i  0  (+  i  1))) 

( (“  i  elements)  new-vector) 

(setf  (aref  new-vector  i)  (abs  (aref  vector  i) ) ) ) ) ) 


1.3. 3. 7. A  ABS-ARRAY  Auxiliary 
1.3.4  General  matrix  output  functions 
1.3. 4.1  PRINT-ARRAY 

(defun  print-array  (array-list) 

(cond  ( (listp  array-list)  (print-array-list  array-list)) 


(t  (print-one-array  array-list)))) 


;  1.3. 4.1. A  PRINT-ARRAY  Auxiliary 

(defun  print-array-list  (array-list) 

(cond  ( (null  array-list)  nil) 

(t  (print-array  (car  array-list) ) 

(print-array-list  (cdr  array-list) ) ) ) ) 

(defun  print-one-array  (array) 

(cond  ( (vectorp  array)  (print -vector  array) ) 

(t  (print-2d-array  array) ) ) ) 

(defun  print-2d-array  (2d-array) 

(write-char  #\newline) 

(write-char  #\newline) 

(let  ((rows  (array-dimension  2d-array  0)) 

(columns  (array-dimension  2d-array  1) ) ) 

(do  ( (i  0  (+  i  1)  ) ) 

( (-  i  rows)  nil) 

(do  ( ( j  0  (+  j  1)  )  ) 

( (“  j  columns)  nil) 

(prinl  (aref  2d-array  i  j)  ) 

(write-char  #\3pace  ) ) 

(write-char  #\newline) ) ) ) 

(defun  print-vector  (vector) 

(write-char  #\newline) 

(write-char  #\newline) 

(let  ((elements  (array-dimension  vector  0))) 

(do  ( (i  0  (+  i  1) )  ) 

( (=  i  elements)  nil) 

(prinl  (aref  vector  i) ) 

(write-char  tXspace  ) ) ) ) 

;  1.4  Graphics  functions 

;  1.4.1  Define  graphics  variables 

(defvar  grasp-screen) 

(defvar  grasp-window) 

(defvar  view-frame) 

(defvar  scale-2d) 

(defvar  x-origin-2d) 

(defvar  y-origin-2d) 

(defvar  3cale-3d) 

(defvar  x-origin-3d) 

(defvar  y-origin-3d) 

(defvar  angle-x) 

(defvar  angle-z) 

;  1.4.2  Set  default  variable  values  section 

;  1.4. 2.1  Define  functions  to  set  graphics  variabl 

es 

(defun  construct-view-frame  (angle-x  angle-z) 

(let*  ( (ct  (cos  angle-x)) 

(St  (sin  angle-x) ) 

(cp  (cos  angle-z)) 

(sp  (sin  angle-z)) 

(view-frame  (zero-array  34))) 

(setf  (aref  view-frame  0  0)  cp) 

(setf  (aref  view-frame  0  1)  (*  -1  ct  sp) ) 

(setf  (aref  view-frame  0  2)  (*  -1  st  sp) ) 

(setf  (aref  view-frame  1  0)  sp) 

(setf  (aref  view-frame  1  1)  (*  ct  cp) ) 

(setf  (aref  view-frame  1  2)  (*  st  cp) ) 

(setf  (aref  view-frame  2  0)  0) 


(setf  (aref  view-frame  2  1)  (*  -1  at) ) 

(setf  (aref  view-frame  2  2)  ct) 
view-frame) ) 


;  1.4. 2. 2  Set  graphics  variables 

(setq  3cale-2d  100) 

(setq  x-origin-2d  300) 

(setq  y-origin-2d  300) 

(setq  scale-3d  200) 

(setq  x-origin-3d  300) 

(setq  y-origin-3d  500) 

(setq  angle-x  0.400) 

(setq  angle-z  0.400) 

(setq  view-frame  (construct -view-frame  angle-x  angle-z)) 


;  1.4.3  Graphics  functions 

;  1.4. 3.1  Screen  definition  and  initialization  fun 

ctions 

;  1.4. 3. 1.1  MAKE-GRASP-SCREEN 

(defun  make-grasp-screen  (Soptional  (proc-msg  t) ) 

(if  (create-grasp-screen) 

(progn  (send  grasp-screen  : activate) 

(send  grasp-screen  rexpose) 

(send  grasp-screen  :send-pane  'top-pane  :select)) 

(progn  (send  grasp-screen  rselect) 

(send  grasp-screen  ;send-pane  'top-pane  :select))) 

(if  proc-msg  ( start -monitor-msg) ) ) 


(defun 

(if 


1.4. 3. 1.2  KILL-GRASP-SCREEN 

kill-grasp-screen  () 

(variable-boundp  grasp-screen) 

(progn  (if  (variable-boundp  pc)  (send  pc  :)till)  ) 

(send  (send  grasp-screen  isend-pane  'bottom-pane  rprocess) 
(send  grasp-screen  ;)cill) 

(variable-makunbound  grasp-screen) 

T) 

nil) ) 


: reset) 


this  function  resets  the  bottom  pane  lisp  listener  and  starts  a  the  message  monitor 
use  (monitor-msg  pc  tg  'verbose)  if  you  want  verbose  message  processing. . . 


;  1.4. 3. 1.3  FORCE-KBD-INPUT-STRING 

(defun  f orce-kbd-input-string  (window  string) 

(loop  for  i  from  0  below  (string-length  string) 

do  (send  window  : force-kbd-input  (char-int  (char  string  i) ) ) ) ) 


1 . 4 . 3 . 1 . 4  START-MONITOR-MSG 


(defun  start-monitor-msg  () 

(if  (variable-boundp  pc) 

(progn  (send  (send  grasp-screen  ;send-pane  'bottom-pane  :process)  : reset) 
(send  grasp-screen  :send-pane  'bottom-pane  '  : dear-screen) 

(f orce-kbd-input-string  (send  grasp-screen  :get-pane  'bottom-pane) 

"(monitor-msg  pc  tg)") 


) 


(send  grasp-screen  :send-pane  'bottom-pane  : line-out 
#.(ZL;STRING  "Parallel  connection  doesn't  exist.  Can't  start  message  processor"))) 
T) 


1.4. 3. 1.5  CREATE -GRASP-SCREENt 


(defun  create-grasp-screen  () 

(setq  grasp-screen  (tv:make-window 

' tv ;bordered-constraint-f rame 


' : panes 

'((top-pane  tv : window-pane 

: label  "MAIN  LISP  LISTENER" 

:more-p  nil) 

(bottom-pane  tv : window-pane 

: label  "MESSAGE  PROCESSOR" 

:more-p  nil) 

(grasp-window  tv; window 

: label  "GRASP  WINDOW" 

:activate-p  t) ) 

' : configurations 
' ( (main-config 
( : layout 

(main-config  :row  grasp-window  right-side) 

(right-side  icolumn  top-pane  bottom-pane)) 

( : sizes 

(right-side  (top-pane  :even)  (bottom-pane  :even) ) 
(main-config  (right-side  0.3)  :then  (grasp-window  :even) ) ) )  ! 

)  )  )  ) 


(defun  clearscreen  () 

(send  grasp-screen  :send-pane 


1.4. 3. 2  CLEARSCREEN 
'grasp-window  ' : refresh) ) 


;  1.4. 3. 3  Draw  functions 

;  1.4. 3. 3.1  DRAW-2D-GRASP-WINDOW 

(defun  draw-2d-grasp-window  (starting-point  ending-point) 

(let  ((x-start)  (y-start) 

(x-end)  (y-end) 

(window-height) ) 

(setq  window-height  (send  grasp-screen  isend-pane  'grasp-window  'iheight)) 

(setq  x-start  (round  (+  x-origin-2d  (*  3cale-2d  (aref  starting-point  0))))) 

(setq  y-start  (round  (+  window-height 

(*  -1  (+  y-origin-2d  (*  scale-2d  (aref  starting-point  1) ) ) ) ) ! 

)  ) 

(setq  x-end  (round  (+  x-origin-2d  (*  scale-2d  (aref  ending-point  0))))) 

(setq  y-end  (round  (+  window-height  (*  -1  (+  y-origin-2d  (*  3cale-2d  (aref  ending-po! 
int  1) ) ) )  )  )  ) 

(send  grasp-screen  :send-pane  'grasp-window  ': draw-line  x-start  y-start  x-end  y-end)! 

)  ) 


;  1.4. 3. 3. 2  DRAW-3D-GRASP-WINDOW 

(defun  draw-3d-grasp-window  (starting-point  ending-point) 

(let  ((start  (zero-vector  3)) 

(end  (zero-vector  3)) 

(x-start  nil) 

(y-start  nil) 

(x-end  nil) 

(y-end  nil) 

(window-height  nil) ) 

(setq  window-height  (send  grasp-screen  :send-pane  'grasp-window  ': height ) ) 

(setq  start  (transform-points-global-f rame  view-frame  starting-point) ) 

(setq  end  (transform-points-global-frame  view-frame  ending-point) ) 

(setq  x-start  (round  (+  x-origin-3d  (*  3cale-3d  (aref  start  0))))) 

(setq  y-start  (round  (+  window-height  (*  -1  (+  y-origin-3d  (*  3cale-3d  (aref  start  2! 
))))))) 

(setq  x-end  (round  (+  x-origin-3d  (*  scale-3d  (aref  end  0))))) 

(setq  y-end  (round  (-  window-height  (+  y-origin-3d  (*  scale-3d  (aref  end  2)))))) 

(send  grasp-screen  :send-pane  'grasp-window  ': draw-line  x-start  y-start  x-end  y-end)! 

) ) 


;  1.4. 3. 3. 3  SPHERE-3D-GRASP-WINDOW 

(defun  3phere-3d-grasp-window  (sphere-center  radius) 

(let  ((center  (zero-vector  3)) 

(x-center) 

(y-center) 


( integer-radius ) 

(window-height  nil) ) 

(setq  window-height  (send  grasp-screen  :send-pane  'grasp-window  height ) ) 

(setq  center  (transform-points-global-f rame  view-frame  sphere-center) ) 

(setq  x-center  (round  (+  x-origin-3d  (*  scale-3d  (aref  center  0))))) 

(setq  y-center  (round  (-  window-height  (+  y-origin-3d  (*  scale-3d  (aref  center  2)))) 

) ) 

(setq  integer-radius  (round  (*  scale-3d  radius) ) ) 

(send  grasp-screen  :send-pane  'grasp-window  ' :draw-filled-in-circle  x-center  y-cente 
r  integer-radius  tv:alu-ior) ) ) 

;  1.4. 3. 3. 4  DRAW-COORDINATES 

(defun  draw-coordinates  (frames  length) 

(cond  ( (listp  frames)  (draw-coordinate-list  frames  length)) 

(t  (draw-coordinate  frames  length) ) ) ) 

;  1.4. 3. 3. 4. A  DRAW-COORDINATES  Auxiliary 

(defun  draw-coordinate-list  (frames  length) 

(cond  (  (null  frsunes)  nil) 

(t  (draw-coordinate  (car  frames)  length) 

(draw-coordinate-list  (cdr  frames)  length) ) ) ) 

(defun  draw-coordinate  (frame  length) 

(let*  (  (o- frame  (ma)ce-array  '(3)  : initial-contents  (list  0  0  0))) 

(x-frame  (make-array  '(3)  : initial-contents  (list  length  00))) 

(y-frame  (make-array  '(3)  : initial-contents  (list  0  length  0))) 

(z-frame  (make-array  '(3)  : initial-contents  (list  0  0  length))) 

(origin-3d  (transform-points-f rame-global  frame  o-frame) ) 

(x  (transform-points-f rame-global  frame  x-frame)) 

(y  (transform-points-f rame-global  frame  y-frame) ) 

(z  (transform-points-f rame-global  frame  z-frame))) 

(draw-3d-grasp-window  origin-3d  x) 

(draw-3d-grasp-window  origin-3d  y) 

(draw-3d-grasp-window  origin-3d  z) ) 

(draw-coordinate-labels  frame  length) ) 

(defun  draw-coordinate-labels  (frame  length) 

(let*  ( (x-point-1  (make-array  '(3)  : initial-contents 

(list  (*  length  1.1)  0  (*  length  -0.05)))) 

(x-point-2  (make-array  ' (3)  : initial-contents 

(list  (*  length  1.17)  0  (*  length  0.05)))) 

(x-point-3  (make-array  ' (3)  : initial-contents 

(list  (*  length  1.1)  0  (*  length  0.05)))) 

(x-point-4  (make-array  ' (3)  : initial-contents 

(list  (*  length  1.17)  0  (*  length  -0.05)))) 

(y-point-1  (make-array  ' (3)  : initial-contents 

(list  0  (*  length  1.1)  (*  length  0.05)))) 

(y-point-2  (make-array  ' (3)  ; initial-contents 

(list  0  (*  length  1.135)  0))) 

(y-point-3  (make-array  ' (3)  : initial-contents 

(list  0  (*  length  1.17)  (*  length  0.05)))' 

(y-point-4  (make-array  ' (3)  : initial-contents 

(list  0  (*  length  1.135)  (*  length  -0.05)))) 

(z-point-1  (make-array  ' (3)  : initial-contents 

(list  (*  length  -0.035)  0  (*  length  1.15)))) 

(z-point-2  (make-array  ' (3)  ; initial-contents 

(list  (*  length  0.035)  0  (*  length  1.15)))) 

(z-point-3  (make-array  ' (3)  : initial-contents 

(list  (*  length  -0.035)  0  (*  length  1.1)))) 

(z-point-4  (make-array  ' (3)  : initial-contents 

(list  (*  length  0.035)  0  (*  length  1.1))))) 

(setq  x-point-1  (transf orm-points-f r£.rae-global  frame  x-point-1) ) 

(setq  x-point-2  (transform-points-f rame-global  frame  x-point-2) ) 

(setq  x-point-3  (transform-f^ints-f rame-global  frame  x-point-3)) 

(setq  x-point-4  (transform-points-f rame-global  frame  x-point-4) ) 

(setq  y-point-1  (transform-points-f rame-global  frame  y-point-1) ) 


(setq  y-point-2  (transform-points-f rame-global  frama  y-point-2) ) 

(aetq  y-point-3  (tranaform-points-f rame-global  frame  y-point-3) ) 

(setq  y-point-4  (transform-points-frame-global  frame  y-point-4) ) 

(setq  z-point-1  (transform-points-frame-global  frame  z-point-l) ) 

(setq  z-point-2  (transform-points-frame-global  frame  z-point-2)) 

(setq  z-point-3  (transform-points-frame-global  frame  z-point-3) ) 

(aetq  z-point-4  (transform-points-frame-global  frame  z-point-4) ) 

(draw-3d-grasp-window  x-point-1  x-point-2) 

(draw-3d-gra3p-window  x-point-3  x-point-4) 

(draw-3d-grasp-window  y-point-1  y-point-2) 

(draw-3d-gra3p-window  y-point-2  y-point-3) 

(draw-3d-gra3p-window  y-point-2  y-point-4) 

(draw-3d-grasp-window  z-point-1  z-point-2) 

(draw-3d-grasp-window  z-point-2  z-point-3) 

(draw-3d-grasp-window  z-point-3  z-point-4) ) ) 

;  1.4. 3. 3. 5  DRAW-3D-LIST 

(defun  draw-3d-list  (list-center  list) 

(let  ( (center  (zero-vector  3) ) 

(x-center) 

(y-center) 

(window-height ) 

(element) 

(n-elements) 

(start) ) 

(setq  window-height  (send  grasp-screen  :send-pane  'grasp-window  height ) ) 

(setq  center  (transf orm-points-global-f rame  view-frame  list-center) ) 

(setq  x-center  (round  (+  x-origin-3d  (*  3cale-3d  (aref  center  0) ) ) ) ) 

(setq  y-center  (round  (-  window-height  (+  y-origin-3d  (*  scale-3d  (aref  center  2))))! 

)  ) 

(setq  n-elements  (count-atoms  list)) 

(do  (  (i  0  (+  i  1) )  ) 

( (-  i  n-elements)) 

(setq  start  (round  (-  x-center  (*  10  (/  n-elements  2))))) 

(setq  element  (car  list)) 

(draw-character  (+  (*  10  i)  start)  y-center  element) 

(setq  list  (cdr  list))))) 

;  1.4. 3. 3. 6  DRAW-CONTACTS-GRASP-SPACE 

(defun  draw-contacts-grasp-space  () 

(setq  view-frame  (construct-view-frame  0.4  0.4)) 

(let*  ( (contact-f rames-grasp-space 

(transform-frames-global-f rame  ‘grasp-frame*  ‘contact-frames*) ) 
(grasp-frame-grasp-space  (make-array  '(3  4)  : initial-contents  '((1  0  0  0) 

(0  10  0) 

(0  0  1  0)))  ! 

)  ) 

(draw-coordinates  (first  contact-f rames-grasp-space)  0.5) 

(draw-coordinates  (second  contact-f rames-grasp-space)  0.5) 

(draw-coordinates  (third  contact-f rames-grasp-space)  0.5) 

(draw-coordinates  grasp-frame-grasp-space  1.0))) 


;  1 . 4 . 3 . 3 . 7  DRAW-CHARACTER 

(defun  draw-character  (x  y  char) 

(cond  ( (numberp  char)  (send  grasp-screen  :send-pane  'grasp-window 

' :draw-string  (+  48  char)  x  y) ) 

(t  (send  grasp-screen  :3end-pane  'grasp-window  ': draw-string  char  x  y) ) ) ) 


2.  Sensed  and  global  variables 


*  *  ★  ★  * 


1 


2.0  Tezaporary  vaxiables  uaed  to  construct  globa 


(defvar 

(defvar 

(defvar 

(defvar 

(defvar 

(defvar 


sensed  variables  for  simulation 
2.0.1  Define  ten^orary  variables 
*original-contact-points-object-space*) 

*original-contact-nonnals-ob ject-space*) 

*original-ob ject-f rame* ) 

*contact-points-ob ject-space*) 

*contact-normal3-ob ject-space*) 

♦object -frame*) 


(defvar  *hand-f rame*) 

(defvar  ‘grasp-frame*) 

;  2.0.2  Set  default  temporary  variables 

;  2. 0.2.1  Functions  to  set  default  values 


(defun  set-original-values  () 

(setq  *original-contact-point3-ob ject-space* 


(list  (ma)te-array 

'  (3) 

: initial-contents 

'  (0 

1.3  -0.8) ) 

(ma  Ice-array 

'  (3) 

; initial-contents 

'  (0 

1.3  0.8)) 

(make-array 

'  (3) 

: initial-contents 

'  (0 

-1.3  0) ) ) ) 

(setq  ‘original-contact- 

-normals-ob ject-space* 

(list  (make-array 

'  (3) 

:  initial-contents 

'  (0 

1  0)) 

(make-array 

'  (3) 

: initial-contents 

'  (0 

1  0)) 

(make-array 

'  (3) 

: initial-contents 

'  (0 

-1  0) ) ) ) ) 

(set -original -values) 

2. 0.2. 2 

Set 

temporary 

variables 

(setq  ‘hand-frame*  (make-. 

array 

'(3  4)  : initial-contents  '((1  0 

0 

0) 

(0  1 

0 

0) 

(0  0 

1 

0) ) )  ) 

(setq  *original-ob ject-frame*  (ma)ce-array  '(3  4)  .'initial-contents  '((0  0  1  -0.8) 

(100  2.7) 
(010  -3.0)))) 

(setq  *contact-point3-ob ject-space*  *otiginal-contact-points-ob ject-space*) 

(setq  *rontact-noriiial3-ob ject-space*  *original-contact-normals-ob ject-space*) 

(setq  *ob jfect-f rame*  *original-ob ject-f rame*) 

;  2.1  Define  sensed  global  variables 

(defvar  *contact -points*) 

(defvar  ‘contact-normals*) 

(defvar  *contcCt -frames* ) 


;  2.2  Set  defualt  initial  global  variables 

;  2.2.1  Functions  to  set  initial  global  variables 

(defun  generate-grasp-f rame  () 


(let*  ( (xl 

(aref 

(first  ‘contact-frames*) 

0 

3)) 

(yl 

(aref 

(first  ‘contact-frames*) 

1 

3)  ) 

(zl 

(aref 

(first  ‘contact-frames*) 

2 

3)  ) 

(x2 

(aref 

(second  ‘contact-frames*) 

0 

3)) 

(y2 

(aref 

(second  ‘contact-frames*) 

1 

3)  ) 

(z2 

(aref 

(second  ‘contact-frames*) 

2 

3)) 

(x3 

(aref 

(third  ‘contact-frames*) 

0 

3) ) 

<y3 

(aref 

(third  ‘contact-frames*) 

1 

3)) 

(z3 

(aref 

(third  ‘contact-frames*) 

2 

3)) 

(centroid 

(make-array  '  (3) ) ) 

(normal  (make-array  '(3)))) 

(setf  (aref  centroid  0)  (/  (+  xl  x2  x3)  3)) 

(setf  (aref  centroid  1)  (/  (+  yl  y2  y3)  3)) 

(setf  (aref  centroid  2)  (/  (+  zl  z2  z3)  3)) 

(setf  (aref  normal  0)  (-  (*  (-  y3  yl)  (-  z2  zl))  (*  {-  z3  zl)  (-  y2  yl)))) 

(setf  (aref  normal  1)  (-  (*  (-  z3  zl)  {-  x2  xl) )  (*  (-  x3  xl)  (-  z2  zl)))) 

(setf  (aref  normal  2)  (-  (*  (-  x3  xl)  (-  y2  yl))  (*  (-  y3  yl)  (-  x2  xl)))) 

(generate-f rame  centroid  normal) ) ) 


(defun  normalize-contact-normals  (contact -normals) 

(cond  ( (null  contact-normals)  nil) 

(t  (cons  (normalize-contact-normal  (car  contact -normals) ) 

(normalize-contact-normals  (cdr  contact-normals)))))) 

(defun  normalize-contact-normal  (contact-normal) 

(let  ( (magnitude  (sqrt  (+  (sqr  (aref  contact -normal  0) ) 

(sqr  (aref  contact -normal  1) ) 

(sqr  (aref  contact -normal  2)))))) 

(setf  (aref  contact-normal  0)  (/  (aref  contact-normal  0)  magnitude) ) 

(setf  (aref  contact-normal  1)  (/  (aref  contact-normal  1)  magnitude) ) 

(setf  (aref  contact-normal  2)  (/  (aref  contact-normal  2)  magnitude) ) 

contact -normal) ) 

(defun  normalize-ob ject-f rame  (object-frame) 

(let  ((magnitude  (zero-vector  3))) 

(do  (  (i  0  (+  i  1)  )  ) 

(  (“  i  3)  ) 

(setf  (aref  magnitude  i)  (sqrt  (+  (sqr  (aref  object-frame  0  i) ) 

(sqr  (aref  object-frame  1  i) ) 

(sqr  (aref  object-frame  2  i) ) ) ) ) 

(do  ( ( j  0  (+  j  1) ) ) 

( (=  j  3)  ) 

(setf  (aref  object-frame  j  i)  (/  (aref  object-frame  j  i)  (aref  magnitude  i) ) ) ) ) ) 
object-frame ) 

;  2.2.2  Set  global-variables 

(defun  initialize-global-variables  () 

(setq  *cont act -norma Is -object -space* 

(normalize-contact-normals  *contact-normals-ob ject-space*) ) 

(setq  *ob ject-f rame*  (normalize-ob ject-f rame  •object-frame*)) 

(setq  *contact-points*  (transf orm-points-f rame-global  *ob ject-f rame* 

*contact-points-ob ject-space*) ) 

(setq  *contact-normal3*  (transform-direction-frame-global  *ob ject-f rame* 

•contact-normals-object-space*) ) 

(setq  *contact-f rames*  (generate-f rame  *contact-points*  *contact-normals*) ) 

(setq  *grasp-f rame*  (generate-grasp-f rame) ) ) 

(initialize-global-variables) 

it  it  if  If  it 

;  3.  Constraint 


;  3.1  Define  constraint  variables 

(defvar  *ba3is-wrenche3*) 

(defvar  *basis-wrench* ) 

(defvar  *virtual-wor)« * ) 

(defvar  *contact-types*) 

;  3.2.1  Set  defualt  variable  value  section 

;  3. 2. 1.1  Functions  for  setting  global  variables 

(defun  construct-basis-wrench  (contact-frames) 

(cond  ( (null  contact-frames)  nil) 

(t  (cons  (transform-basis-wrench  *basis-wrench*  (car  contact-frames) ) 

(construct -basis-wrench  (cdr  contact-frames) ) ) ) ) ) 

(defun  transform-basis-wrench  (basis-wrench  contact-frame) 

(let*  ( (transformed-basis-wrench 


(transform-direction-frame-global  contact -frame  basis-wrench) ) 
(X  (aref  contact-frame  0  3) ) 

<y  <aref  contact-frame  1  3) ) 

(z  (aref  contact-frame  2  3))) 

(do  ( (i  0  (+  1  i> ) ) 

( (-  i  11)  transformed-basis-wrench) 

(setf  (aref  transformed-basis-wrench  3  i) 

(+  (*  (aref  transformed-basis-wrench  1  i)  z  -1) 

(*  (aref  transformed-basis-wrench  2  i)  y) 

(aref  transformed-basis-wrench  3  i))) 

(setf  (aref  transformed-basis-wrench  4  i) 

(+  (*  (aref  transformed-basis-wrench  2  i)  x  -1) 

(*  (aref  transformed-basis-wrench  0  i)  z) 

(aref  transformed-basis-wrench  4  i) ) ) 

(setf  (aref  transformed-basis-wrench  5  i) 

(+  (*  (aref  transformed-basis-wrench  0  i)  y  -1) 

(*  (aref  transformed-basis-wrench  1  i)  x) 

(aref  transformed-basis-wrench  5  i) ) ) ) ) ) 

;  3. 2. 1.2  Set  constraint  variables 

(setq  *contact-type3*  (list  (ma)ce-array  '  (12)  : initial-contents 

'(1  1101111000  D) 
(ma)ce-array  '  (12)  : initial-contents 

'(1  1100011000  0)) 
(make-array  '  (12)  : initial-contents 

'(001000000000)) 
(make-array  '  (12)  : initial-contents 

'(0  0000000000  0)))) 


(setq  *ba3is-wrench*  (make-array  '(6  12)  : initial-contents 


) 


'((100000-100000) 

(0100000-10000) 

(0  0100000  -1  00  0) 

(0  00100000  -1  0  0) 
(0000100000-10) 

(0  0000100000  -1))) 


(setq  ‘basis-wrenches*  (construct -basis-wrench  ‘contact-frames*) ) 


;  3.3  Constraint  functions 

;  3.3.1  CONSTRUCT -VIRTUAL-WORK-LIST 

(defun  construct -virtual-work-list  (basis-wrenches  twist) 

(cond  ( (null  basis-wrenches)  nil) 

(t  (cons  (construct-virtual-work  (car  basis-wrenches)  twist) 

(construct-virtual-work-list  (cdr  basis-wrenches)  twist) ) ) ) ) 


;  3.3.1. A  CONSTRUCT-VIRTUAL-WORK-LIST  Auxiliary 

(defun  construct-virtual-work  (basis-wrench  twist) 

(let  ((virtual-work-vector  (make-array  '(12)))) 

(do  ((i  0  (+  1  i))) 

( («  i  12)  virtual-work-vector) 

(setf  (aref  virtual-work-vector  i) 

(virtual-work  (get-array-column  basis-wrench  i)  twist) ) ) ) ) 


(defun  virtual-work (wrench  twist) 

(+  (‘  (aref  wrench  0)  (aref  twist  3)) 

(*  (aref  wrench  1)  (aref  twist  4) ) 

(*  (aref  wrench  2)  (aref  twist  5)) 

(*  (aref  wrench  3)  (aref  twist  0)) 

(*  (aref  wrench  4)  (aref  twist  1)) 

(*  (aref  wrench  5)  (aref  twist  2)))) 


3.3.2  DETERMINE-CONT ACT -TYPES 


(defun  deterndne-contact-types  (virtual-worJc-pattern-liat) 

(cond  ( (null  virtual-wor)c-pattern-list)  nil) 

(t  (cons  (detemine-contact-type  (car  virtual-work-pattern-list)  ‘contact-types* 


) 


(determine-contact-types  (cdr  virtual-work -pattern-list) )))) ) 


3. 3. 2. A  DETERMINE-CONTACT-TYPES  Auxiliary 


(defun  determine-contact-type  (virtual-work-pattern  contact -types) 

(cond  ((null  contact-types)  nil) 

(t  (cons  (test-slip  virtual-work-pattern  (car  contact -types) ) 

(determine-contact-type  virtual-work-pattem  (cdr  contact -types) ) ) ) ) ) 

(defun  test-slip  (virtual-work-pattern  contact -type-pattern) 

(let  ( (total  0) ) 

(do  ( (i  0  (+  1  i) ) ) 

(  (-  i  12)  (-  total  0)  )' 

(setq  total  (+  total  (*  (aref  virtual-work-pattern  i) 

(aref  contact-type-pattern  i))))))) 

(defun  construct-virtual-work-pattern-list  (virtual-work-list) 

(cond  ( (null  virtual-work-list)  nil) 

(t  (cons  (construct-virtual-work-pattern  (car  virtual-work-list) ) 

(construct-virtual-work-pattern-list  (cdr  virtual-work-list) ) ) ) ) ) 

(defun  construct-virtual-work-pattern  (virtual-work) 

(let  ((virtual-work-pattern-vector  (make-array  '(12)))) 

(do  ( (i  0  (+  1  i) ) ) 

( (-  i  12)  virtual-work -pattern-vector) 

(cond  ( (>•  (aref  virtual-work  i)  0)  (setf  (aref  virtual-work-pattern-vector  i)  0)) 
(t  (setf  (aref  virtual-work-pattern-vector  i)  1)))))) 


(defun  constraint-state  (twist  twist-ref) 

(constraint-state-aux  (constraint  twist  twist-ref) ) ) 

(defun  constraint-state-aux  (states) 

(cond  ((null  states)  nil) 

(t  (cons  (constraint-state-aux-1  (car  states)) 

(constraint-state-aux  (cdr  states)))))) 


(defun  constraint-state-aux-l  (state) 

(cond  ( (car  state)  1) 

( (+  1  (constraint-state-aux-1  (cdr  state)))))) 

(defun  constraint (twist  twist-ref) 

(let  ( (twist-origin  (twist-org  twist  twist-ref) ) ) 
(determine-contact-types 

( const ruct-virtual-work-pattern-list 

(construct-virtual-work-list  ‘basis-wrenches*  twist-origin) ) ) ) ) 

(defun  twist-org  (twist  twist-ref) 

(let  ((twist-org  (make-array  '(6))) 

(rx  (*  -1  (aref  twist-ref  0))) 

(ry  (*  -1  (aref  twist-ref  1))) 

(rz  (*  -1  (aref  twist-ref  2)))) 


(setf 

(aref 

twist-org 

0) 

(aref  twist  0)) 

(setf 

(aref 

twist-org 

1) 

(aref  twist  1)) 

(setf 

(aref 

twist-org 

2) 

(aref  twist  2)) 

(setf 

(aref 

twist-org 

3) 

(+  (aref  twist  2) 

(*  (aref  twist  1) 

rz) 

(‘  (aref  twist  2) 

ry  -1))) 

(setf 

(aref 

twist-org 

4) 

(+  (aref  twist  4) 

(*  (aref  twist  2) 

rx) 

(*  (aref  twist  0) 

rz  -1))) 

(setf  (aref  twist-org  5)  (+  (aref  twist  5) 

(*  (aref  twist  0)  ry) 

(*  (aref  twist  1)  rx  -1))) 

twist-org) ) 

;  3.4  Geometric  Constraint 

;  3.4.1  DRAW-CONTACT-TRAJECTORY 

(defun  draw-contact-trajectory  (contact-point  twist  twist-ref  twist -magnitude  steps) 
(let  ( (new-contact -point  (zero-vector  3))) 

(do  ((i  1  (+  i  1))) 

( (-  i  steps) ) 

(setq  new-contact -point 

(determine-contact-pciint  contact-point  twist  twist-ref 

(/  (*  i  twist-magnitude)  steps))) 
(draw-3d-gra3p-window  contact -point  new-contact -point) 

(setq  contact -point  new-contact-point)))) 


(defun  determine-contact -point 

(let*  ((rotation-matrix  (zero-array  3 
(twist-radius  (zero-vector  3)) 
(translation  (zero-vector  3) ) 
(new-contact -point  (zero-vector 


3. 4.1. A  DRAW-CONTACT-TRAJECTORY  Auxiliary 
(contact -point  twist  twist-ref  twist -magnitude) 


3)) 


3>) 


(tl 

(t2 

(t3 

(t4 

(tS 

(t6 

(cn 


(aref  twist 
(aref  twist 
(aref  twist 
(aref  twist 
(aref  twist 
(aref  twist 


0) ) 
1) ) 
2)  ) 

3)  ) 

4)  ) 

5)  ) 


(cos  twist-magnitude) ) 


(sn  (sin  twist-magnitude) ) ) 

(setf 

(aref 

rotation-matrix 

0 

0) 

(  + 

(*  tl  tl  (-  1 

cn) ) 

cn)  ) 

(setf 

(aref 

rotation-matrix 

0 

1) 

(- 

(*  t2  tl  (-  1 

cn) ) 

(*  t3 

sn) ) ) 

(setf 

(aref 

rotation-matrix 

0 

2) 

(+ 

(*  t3  tl  (-  1 

cn) ) 

(*  t2 

sn) ) ) 

(setf 

(aref 

rotation-matrix 

1 

0) 

(  + 

(*  tl  t2  (-  1 

cn) ) 

(*  t3 

sn) ) ) 

(setf 

(aref 

rotation-matrix 

1 

1) 

(  + 

(*  t2  t2  (-  1 

cn) ) 

cn) ) 

(setf 

(aref 

rotation-matrix 

1 

2) 

(- 

(*  t3  t2  (-  1 

cn) ) 

(*  tl 

sn)  ) ) 

(setf 

(aref 

rotation-matrix 

2 

0) 

(- 

{*  tl  t3  (-  1 

cn) ) 

(*  t2 

sn) ) ) 

(setf 

(aref 

rotation-matrix 

2 

1) 

(  + 

(*  t2  t3  (-  1 

cn) ) 

(*  tl 

sn) ) ) 

(setf 

(aref 

rotation-matrix 

2 

2) 

{  + 

(*  t3  t3  (-  1 

cn) ) 

cn) ) 

(setf 

(aref 

twist-radius  0) 

(• 

-  (aref 

contact -point 

0) 

(aref 

twist-ref 

(setf 

(aref 

twist-radius  1) 

(■ 

-  (aref 

contact-point 

1) 

(aref 

twist-ref 

(setf 

(aref 

twist-radius  2) 

(■ 

-  (aref 

contact -point 

2) 

(aref 

twist-ref 

(setf 

(aref 

translation  0) 

(* 

twist- 

magnitude  t4) ) 

(setf 

(aref 

translation  1) 

(* 

twist- 

magnitude  t5) ) 

(setf 

(aref 

translation  2) 

(* 

twist- 

magnitude  1 6 ) ) 

(setq  new-contact -point 


(add-array-list  (list 
(multiply-array-list  (list 
twist-ref  translation) ) ) ) ) 


0))) 

1))) 

2))) 


rotation-matrix  twist-radius) ) 


it  it  it  it  it 


;  4.  Body  wrench 

*ir*iHt 


;  4.1  Define  body  wrench  variables 

(defvar  *body-wrench*) 

(defvar  *ob ject-mass*) 

(defvar  ‘gravitational-acceleration*) 


(defun  construct-body-wrench  0 


4.2  Set  body  wrench  variables  section 
4.2.1  Auxiliary  body  wrench  functions 


(let  ( (body-wrench  (zero-vector  6) ) 

(weight  (*  ‘object-mass’*  ‘gravitational-acceleration*))) 

(setf  (aref  body-wrench  2)  weight) 

(setf  (aref  body-wrench  3)  (‘  (aref  ‘object-frame*  1  3)  weight)) 
(setf  (aref  body-wrench  4)  (‘  -1  (aref  ‘object-frame*  0  3)  weight)) 
body- wrench) ) 


;  4.2.2  Set  body  wrench  variables 

(setq  ‘object-mass*  0) 

(setq  ‘gravitational-acceleration*  32.2) 

(setq  ‘body-wrench*  (construct -body-wrench) ) 


;  4.3  Body  wrench  functions 

;  4.3.1  ORIENTATION 

(defun  orientation  (twist-cg) 

(let*  ( (t3  (aref  twist-cg  3)) 

(t4  (aref  twist-cg  4) ) 

(t5  (aref  twist-cg  5) ) 

(twist-magnitude  (sqrt  (abs  (+  (‘  t3  t3)  (‘  t4  t4)  (*  t5  t5) ) ) ) ) 
(theta  (asin  (/  t3  twist-magnitude))) 

(phi  (asin  (/  (*  -1  t4)  twist-magnitude)))) 

(list  theta  phi) ) ) 

4.3.2  TWIST-CG 

(de-<’un  cwist-cg  (twist  twist-ref  eg) 

(let  ((twist-cg  (make-array  '(€))) 

(rx  (-  (aref  eg  0)  (aref  twist-ref  0))) 

(ry  (-  (aref  eg  1)  (aref  twist-ref  1))) 

(rz  (-  (aref  eg  2)  (aref  twist-ref  2)))) 


(setf 

(aref 

twist-cg 

0) 

(aref 

twist 

0)) 

(setf 

(aref 

twist-cg 

1) 

(aref 

twist 

1) ) 

(setf 

(aref 

twist-cg 

2) 

(aref 

twist 

2)) 

(setf 

(aref 

twist-cg 

3) 

(  + 

(aref  twist  2) 

(* 

(aref 

twist 

1) 

rz) 

(* 

(aref 

twist 

2) 

ry  -1))) 

(setf 

(aref 

twist-cg 

4) 

(  + 

(aref  twist  4) 

(aref 

twist 

2) 

rx) 

(* 

(aref 

twist 

0) 

rz  -1))) 

(setf 

(aref 

twist-cg 

5) 

(  + 

(aref  twist  5) 

(* 

(aref 

twist 

0) 

ry) 

{* 

(aref 

twist 

1) 

rx  -1))) 

twist-cg) ) 

■k  h-k  it  it 

t 

;  5.  Contact  Wrenches 
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;  5.1  Define  contact  wrench  variables 

(defvar  ‘fingertip-stif fness*) 

(defvar  ‘finger-stiffness*) 

(defvar  ‘contact-stiffness*) 

(defvar  *contact-st if fness-cpntact -frame*) 

(defvar  *contact-stif fness-hand-frame*) 

(defvar  ‘grasp-stiffness*) 

(defvar  ‘grasp-compliance*) 


(defvar  ‘contact-wrench*) 

(defvar  ‘contact-wrench-contact-f rame*) 


5.2  Set  contact  wrench  variables  section 


;  5.2.1  Functions  to  set  contact  wrench  variables 

(defun  construct-contact-wrench-variables  () 

(setq  *contact-stif fness*  (construct-contact-stiffness  *fingertip-stiffness* 

*finger-stiffness* 
‘contact-frames*) ) 

(setq  *contact-3tif fness-contact-f rame*  (construct-contact-stif fness-contact-f rame 

‘contact-stiffness* 

‘contact-frames*) ) 

(setq  ‘contact-stiffness-hand-frame*  (construct-contact-stiffness-hand-frame 

‘contact-stiffness* 

‘contact-frames*) ) 

(setq  ‘grasp-stiffness*  (add-array-list  ‘contact-stiffness-hand-frame*)) 

(setq  ‘grasp-compliance*  (math; invert -matrix  ‘grasp-stiffness*)) 

(setq  ‘contact-wrench*  (construct-contact-wrenches  ‘contact-stiffness-contact-frame* 

‘offset-wrench*) ) 

( setq  ‘contact-wrench-contact-frame* 

(construct-contact-wrenches-contact-freune  ‘contact-wrench*  ‘contact-frames*) ) ) 

;  5. 2. 1.1  Contact  stiffness 

(defun  construct-contact-stif fness  (fingertip-stiffness  finger-stiffness  contact-frames) 
(cond  ( (null  contact-frames)  nil) 

(t  (cons  (construct-one-contact-stiffness  (car  fingertip-stiffness) 

(car  finger-stiffness) 

(car  contact -frames) ) 
(construct-contact-stiffness  (cdr  fingertip-stiffness) 

(cdr  finger-stiffne..  s) 

(cdr  contact-frames) ) ) ) ) ) 

(defun  construct-one-contact-stiffness  (fingertip-stiffness  finger-stiffness  contact-fra 
me) 

(cond  (  (null  finger-stiffness)  (abs-array  (transform-direction-frame-global 

contact-frame  fingertip-stiffness) ) ) 

(t  (add-stiffness  finger-stiffness 

(abs-array  (transform-direction-frame-global 

contact-frame  fingertip-stiffness) ) ) ) ) ) 


(defun  add-stiffness  (stiffness-1  stiffness-2) 

(let*  ((stiffness-dimension  (array-dimension  stiffness-l  0) ) 
(new-stif fness  (zero-vector  stiffness-dimension) ) ) 

(do  ((i  0  (+  i  1))) 

( (-  i  stiffness-dimension)  new-stiffness) 

(setf  (aref  new-stiffness  i)  (/  (*  (aref  stiffness-1  i) 

(+  (aref  stiffness-l  i) 


(aref  stiffness-2  i) ) 

(aref  stiffness-2  i) ) ) ) ) ) ) 


;  5. 2. 1.2  Contact  stiffness  contact  frame 

(defun  construct-contact-stiffness-contact-frame  (contact-stiffness  contact-f r’^mes) 
(cond  ( (null  contact-frames)  nil) 

(t  (cons  (construct-one-contact-stiffness-contact-frame  (car  contact-stiffness) 

(car  contact-frames) ) 

(construct-contact-stif fness-contact-f rame  (cdr  contact-stiffness) 

(cdr  contact-frames) ) ) ) ) ) 

(defun  construct-one-contact-stiffness-contact-frame  (contact-stiffness  contact-frame) 
(let  ((rx  (aref  contact-frame  0  3)) 

(ry  (aref  contact-frame  1  3)) 

(rz  (aref  contact-frame  2  3) ) 

(Itx  (aref  contact-stiffness  0)) 

()ty  (aref  contact-stiffness  1)) 

(kz  (aref  contact-stiffness  2)) 

()<tx  (aref  contact-stiffness  3)) 

()cty  (aref  contact-stiffness  4)) 

{ktz  (aref  contact-stiffness  5)) 

■ (contact-stiffness-contact-frame  (zero-array  6  6) ) ) 

(setf  (aref  contact-stiffness-contact-f rame  0  1)  (*  )cx  rz) ) 


(aetf  (aref  contact-stiffness-contact-fraine  0  2)  (*  -1  kx  ry) ) 

(aetf  (aref  contact-stiffness-contact-frame  0  3)  kx) 

(setf  (aref  contact-stif fnesa-contact-f rame  1  0)  (*  -1  ky  rz) ) 

(setf  (aref  contact-stiffness-contact-frame  1  2)  (*  ky  rx) ) 

(setf  (aref  contact-stiffness-contact-frame  1  4)  ky) 

(setf  (aref  contact-stiffness-contact-frame  2  0)  (*  kz  ry) ) 

(setf  (aref  contact-stiffness-contact-frame  2  1)  (*  -1  kz  rx) ) 

(setf  (aref  contact-stiffness-contact-frame  2  5)  kz) 

(setf  (aref  contact-stiffness-contact-frame  3  0)  ktx) 

(setf  (aref  contact-stif fness-contact-frame  4  1)  kty) 

(setf  (aref  contact-stiffness-contact-f rame  5  2)  ktz) 

contact-stiffness-contact-frame) ) 

;  5.2. 1.3  Contact  stiffness  hand  frame 

(defun  construct-contact-stiffness-hand-frame  (contact-stiffness  contact-frames) 
(cond  ((null  contact-frames)  nil) 

(t  (cons  (construct-one-contact-stiffness-hand-frame  (car  contact-stiffness) 

(car  contact-frames) ) 

(construct-contact-stiffness-hand-frame  (cdr  contact-stiffness) 

(cdr  contact-frames) ) ) ) ) ) 

(defun  construct-one-contact-stiffness-hand-frame  (contact-stiffness  contact-frame) 
(let  ( (rx  (aref  contact-frame  0  3)) 

(ry  (aref  contact-frame  1  3)) 

(rz  (aref  contact-frame  2  3) ) 

(kx  (aref  contact-stiffness  0)) 

(ky  (aref  contact-stiffness  1)) 

(kz  (aref  contact-stiffness  2) ) 

(ktx  (aref  contact-stiffness  3) ) 

(kty  (aref  contact-stiffness  4) ) 

(ktz  (aref  contact-stiffness  5) ) 

(contact-stiffness-hand-frame  (zero-array  6  6) ) ) 

(setf  (aref  contact-stiffness-hand-frame  0  1)  (*  kx  rz) ) 

(setf  (aref  contact-stiffness-hand-frame  0  2)  (*  -1  kx  ry) ) 

(setf  (aref  contact-stiffness-hand-frame  0  3)  kx) 

(setf  (aref  contact-stiffness-hand-frame  1  0)  (*  -1  ky  rz) ) 

(aetf  (aref  contact-stiffness-hand-frame  1  2)  (*  ky  rx) ) 

(setf  (aref  contact-stiffness-hand-frame  1  4)  ky) 

(setf  (aref  contact-stiffness-hand-frame  2  0)  (*  kz  ry) ) 

(setf  (aref  contact-stiffness-hand-frame  2  1)  (*  -1  kz  rx) ) 

(setf  (aref  contact-stiffness-hand-frame  2  5)  kz) 

(setf  (aref  contact-stiffness-hand-frame  3  0)  (+  (*  ky  rz  rz)  (*  kz  ry  ry)  ktx) ) 

(setf  (aref  contact-stiffness-hand-frame  3  1)  (*  -1  kz  rx  ry) ) 

(setf  (aref  contact-stiffness-hand-frame  3  2)  (*  -1  ky  rx  rz) ) 

(setf  (aref  contact-stiffness-hand-frame  3  4)  (*  -1  ky  rz) ) 

(setf  (aref  contact-stiffness-hand-frame  3  5)  (*  kz  ry) ) 

(aetf  (aref  contact-stiffness-hand-frame  4  0)  (*  -1  kz  rx  ry) ) 

(setf  (aref  contact-stiffness-hand-frame  4  1)  (+  (*  kx  rz  rz)  (*  kz  rx  rx)  kty) ) 

(setf  (aref  contact-stiffness-hand-frame  4  2)  (*  -1  kx  ry  rz) ) 

(setf  (aref  contact-stiffness-hand-frame  4  3)  (*  kx  rz) ) 

(setf  (aref  contact-stiffness-hand-frame  4  5)  (*  -1  kz  rx) ) 

(setf  (aref  contact-stiffness-hand-frame  5  0)  (*  -1  ky  rx  rz) ) 

(setf  (aref  contact-stiffness-hand-frame  5  1)  (*  -1  kx  ry  rz) ) 

(setf  (aref  contact-stiffness-hand-frame  5  2)  (+  (*  kx  ry  ry)  (*  ky  rx  rx)  ktz)) 

(setf  (aref  contact-stiffness-hand-frame  5  3)  (*  -1  kx  ry) ) 

(setf  (aref  contact-stiffness-hand-frame  5  4)  (*  ky  rx) ) 

contact-stiffness-hand-frame) ) 

5. 2. 1.3  Grasp  stiffness 

5. 2. 1.4  Grasp  con^liance 

5. 2. 1.5  Contact  wrench 

(defun  construct-contact-wrenches  (contact-stiffness-contact-frame  offset-wrench) 
(cond  ( (null  contact-stiffness-contact-frame)  nil) 

(t  (cons  (construct-contact-wrench  (car  contact-stiffness-contact-frame) 

(car  offset-wrench) ) 

( const ruct-contact-wrenches  (cdr  contact-stiffness-contact-f rame) 

(cdr  offset-wrench) ) ) ) ) ) 


(defun  construct-contact-wrench  (contact-stlffness-contact-freune  offset-wrench) 
(add-array-list  (list  (multiply-array-list 

(list 

(multiply-array-list 

(list  contact-stiffness-contact-frame  *graap-con?>liance*) ) 
*body-wrench*) ) 
offset-wrench) ) ) 


;  5. 2. 1.5  Contact  wrench  contact  frame 

(defun  construct-contact-wrenches-contact-frame  (contact -wrenches  contact-frames) 
(cond  ( (null  contact-wrenches)  nil) 

(t  (cons  (transform-direction-global-frame  (car  contact-frames) 

(car  contact -wrenches) ) 

(construct-contact-wrenches-contact-f rame  (cdr  contact -wrenches) 

(cdr  contact-frames) ) ) ) ) ) 


(setq  ‘fingertip-stiffness* 


) 


(list 


5.2.2  Set 
(make-array  ' (6) 
(make-array  ' (6) 
(make-array  ' (6) 


contact  wrench  variables 
: initial-contents  '  (10  10  25  0 
: initial-contents  ' (10  10  25  0 
: initial-contents  ' (10  10  25  0 


0  6)  ) 

0  6)) 

0  6) )  )  ! 


(setq  ‘finger-stiffness*  (list  nil  nil  nil)) 
( const ruct-contact -wrench- variables) 


it  it  it  it  it 


;  6.  Contact-types 

it  it  it  it  it 


1  6.1  Define  contact  type  variables 

(defvar  ‘f ric-coef f ‘) 

(defvar  ‘mom-coeff‘) 

;  6.2  Set  contact  type  variables  section 

(setq  ‘f ric-coef f*  0.6) 

(setq  ‘mom-coeff‘  0.1) 

;  6.3  Contact  type  functions 

;  6.3.1  CONTACT-TYPE 

(defun  contact-type  (contact -wrenches) 

(cond  ( (null  contact-wrenches)  nil) 

(t  (cons  (contact-type-aux  (car  contact -wrenches) ) 

(contact-type  (cdr  contact-wrenches)))))) 

;  6. 3.1. A  Auxiliary  contact  type  functions 

(defun  contact-type-aux  (contact -wrench) 

(let*  ((normal-force  (aref  contact -wrench  2)) 

(tangent-force  (sqrt  (+  (*  (aref  contact-wrench  0)  (aref  contact-wrench  0)) 

(*  (aref  contact -wrench  1)  (aref  contact-wrench  1))))) 
(moment  (abs  (aref  contact-wrench  5))) 

(max-tangent-force  (*  *f ric-coef f*  (abs  normal-force))) 

(max-moment  (*  *mom-coeff*  (aba  normal-force)))) 

(cond  ( (>-  normal-force  0)  4) 

( (>“  tangent-force  max-tangent-force)  3) 

( (>"  moment  max-moment)  2) 

(t  1)))) 

.  •  .  kiHc-kifkiticitifkitititititititit1tit*ititit1titit1tititititit*1fk-k1titirHititititit1t1tiiititiiit1tititit1r’kit*ikititiiifk1ritiritii*ititititititltltititit  t 
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7 .  Offset  wrench 
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;  7.1  Define  offset  wrench  variables 

(defvar  *offset -wrench*) 

(defvar  *off set-wrench-contact-frame*) 

(defvar  *scale-force*) 


;  7.2  Set  offset  wrench  defaults 

(setq  *off set-wrench*  (list  (zero-vector  6) (zero-vector  6) (zero-vector  6))) 

(setq  *.of  fset-wrench-contact-frame*  (zero-vector  6) ) 

(setq  *scale-force*  1.0) 

;  7.3  Offset  wrench  functions 

;  7.3.1  CONSTRUCT-OFFSET-WRENCHES 

(defun  construct-off set -wrenches  (force-center  force-magnitude) 

(setq  *off set-wrench*  (offset-wrench-three-contacts  force-center  force-magnitude) ) 

(setq  *of fset-wrench-contact-frame* 

(transform-direction-global-frame  *contact-frames*  *offset-wrench*) ) ) 

;  7. 3.1, A  OFFSET-WRENCH-THREE-CONTACTS 

;  There  are  two  solutions  for  the  forces.  One  solution  in  which  most  of  the  force  vector! 
s  are 

;  directed  into  the  force-centroid  and  other  in  which  two  or  more  force  vectors  are 
;  directed  away  from  the  centroid. 

(defun  of fset-wrench-three-contacts  (force-center  force-magnitude) 

(let*  ((force-array  (zero-array  3  3)) 

(reduced-force-array  (make-array  '(3  3))) 

(offaetl  (zero-vector  6) ) 

(offset2  (zero-vector  6) ) 

(offset3  (zero-vector  6)) 

(length-1)  (length-2)  (length-3) 

(force-1)  (force-2)  (force-3) 

(x  (aref  force-center  0)) 

(y  (aref  force-center  1)) 

(z  (aref  force-center  2) ) 


(a)  (b) ) 

(setq 

length- 1 

(sqrt 

(  + 

(sqr 

(-  X 

(aref 

(first  *contact-frames*) 

0  3))) 

(sqr 

(-  y 

(aref 

(first  *contact -frames*) 

13))) 

(sqr 

(-  z 

(aref 

(first  *contact-frames*) 

2  3) ) ) ) )  ) 

(setq 

length-2 

(sqrt 

(  + 

(sqr 

(-  X 

(aref 

(second  ‘contact-frames*) 

0  3))) 

(sqr 

(-  y 

(aref 

(second  *contact-frames*) 

1  3))) 

(sqr 

(-  z 

(aref 

(second  ‘contact-frames*) 

2  3)))))) 

(setq 

length-3 

(sqrt 

(  + 

(sqr 

(-  X 

(aref 

(third  ‘contact-frames*) 

0  3))) 

(sqr 

(-  y 

(aref 

(third  ‘contact-frames*) 

1  3))) 

(sqr 

(-  Z 

(aref 

(third  ‘conta<jj:-frames‘) 

2  3)))))) 

(setf 

(aref 

force-array 

0 

0) 

(/ 

(-  X 

(aref 

(first  ‘contact-frames*) 

0 

3)) 

length-1) ) 

(setf 

(aref 

force-array 

0 

1) 

(/ 

(-  X 

(aref 

(second  ‘contact-frames*) 

0  3)) 

length-2) 

(setf 

(aref 

force-array 

0 

2) 

{/ 

(-  X 

(aref 

(third  ‘contact -frames*) 

0 

3) ) 

length-3) ) 

(setf 

(aref 

force-array 

1 

0) 

(/ 

(-  y 

(aref 

(first  ‘contact -frames*) 

1 

3) ) 

length-1) ) 

(setf 

(aref 

force-array 

1 

1) 

(/ 

(-  y 

(aref 

(second  ‘contact-frames*) 

1  3)) 

length-2) 

(setf 

(aref 

force-array 

1 

2) 

(/ 

(-  y 

(aref 

(third  ‘contact -frames*) 

1 

3)  ) 

length-3) ) 

(setf 

(aref 

force-array 

2 

0) 

(/ 

(-  z 

(aref 

(first  ‘contact-frames*) 

2 

3)  ) 

length-1) ) 

(setf 

(aref 

force-array 

2 

1) 

(/ 

(-  z 

(aref 

(second  ‘contact-frames*) 

2  3)) 

length-2) 

(setf 

(aref 

force-array 

2 

2) 

(/ 

(-  z 

(aref 

(third  ‘contact-frames*) 

2 

3)  ) 

length-3) ) 

(if  (<  force-magnitude  0)  (setq  force-array  (multiply-array-list  (list  -1  force-arra! 
y) )  ) ) 


(setq  reduced-force-array  (row-reduced-echelon  force-array) ) 

(setq  a  (aref  reduced-force-array  02)) 

(setq  b  (aref  reduced-force-array  1  2) ) 

(setq  fofce-3  (sqrt  (/  (sqr  force-magnitude)  (+  (sqr  a)  (sqr  b)  1) > ) ) 

(setq  force-2  (*  -1  b  force-3)) 

(setq  force-1  (*  -1  a  force-3)) 

(setf  (aref  offsetl  0)  (*  *scale-force*  force-1  (aref  force-array  0  0))) 

(setf  (aref  offsetl  1)  (*  *scale-force*  force-1  (aref  force-array  1  0))) 

(setf  (aref  offsetl  2)  (»  *3cale-force*  force-1  (aref  force-array  2  0))) 

(setf  (aref  offset2  0)  {*  *scale-force*  force-2  (aref  force-array  0  1))) 

(setf  (aref  offset2  1)  (*  »scale-force*  force-2  (aref  force-array  1  1))) 

(setf  (aref  off3et2  2)  (*  *3cale-force*  force-2  (aref  force-array  2  1))) 

(setf  (aref  off3et3  0)  (*  *scale-force*  force-3  (aref  force-array  02))) 

(setf  (aref  offset3  1)  (*  *3cale-force*  force-3  (aref  force-array  12))) 

(setf  (aref  offset3  2)  (*  *scale-force*  force-3  (aref  force-array  22))) 

(list  offsetl  off3et2  off3et3) ) ) 
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;  8.1  DETERMINE -CONSTRAINT-STATE 

(defun  determine-constraint-state  (force-center  force-magnitude) 

(let  ( (grasp-vector-list) 

(constraint -St ate) 

(label-point 

(transform-points-global-frame  *grasp-f rame*  force-center) ) ) 
(construct-offset-wrenches  force-center  force-magnitude) 

(setq  grasp-vector-list  (construct-grasp-points  force-center  force-magnitude)) 
(construct -contact-wrench-variables) 

(setq  constraint-state  (contact-type  •contact-wrench-contact-frame*)) 
(draw-coordinate-system) 

(3phere-3d-grasp-window  (first  grasp-vector-list)  0.01) 

(sphere-3d-grasp-window  (second  grasp-vector-list)  0.01) 
(sphere-3d-gra3p-window  (third  grasp-vector-list)  0.01) 

(sphere-3d-grasp-window  force-center  0.02) 

(setf  (aref  label-point  1)  (+  (aref  label-point  1)  0.025)) 

(setq  label-point  (transform-points-f rame-global  *grasp-f rame*  label-point)) 
(draw-3d-list  label-point  constraint-state) ) ) 

;  8.2  MAP-CONSTRAINT-SPACE 


(defun  map-constraint-state 

(x-start  x-end  x-inc  y-start  y-end  y-inc  force-magnitude) 

(let  ( (force-center-grasp-frame  (zero-vector  3) ) 

(force-center-hand-frame  (zero-vector  3) ) 

(label-point) 

(constraint-state) ) 

(do  ( (X  x-start  (+  x  x-inc) ) ) 

( (>■  X  x-end) ) 

(do  ( (y  y-start  (+  y  y-inc))) 

( (>-  y  y-end) ) 

(setf  (aref  force-center-grasp-frame  0)  x) 

(setf  (aref  force-center-grasp-frame  1)  y) 

(setq  label-point  force-center-grasp-frame) 

(setq  force-center-hand-frame 

(transform-points-f rame-global  *gra3p-f rame*  force-center-grasp-frame) ) 
(construct-offset-wrenches  force-centet-hand- frame  force-magnitude) 

(construct -contact -wrench- variables) 


(setq  constraint-state  (contact-type  ‘contact -wrench-contact-f rame*) ) 
(aphere-3d-grasp-window  force-center-hand-frame  0.02) 

(set£  (are£  label-point  1)  (+  (are£  label-point  1)  0.025)) 

(setq  label-point  (trans£onn-point3-£ranie-global  ‘grasp-frame*  label-point) ) 
(draw-3d-li3t  label-point  constraint-state) ) ) ) ) 


(defun  map-constraint-space  (force-magnitude) 

(let*  ((point-grasp-frame  (ma)te-array  '(3)  : initial-contents  '(0  0  0))) 

(point -hand-frame  (ma)te-array  '(3)  : initial-contents  '(0  0  0))) 
(constraint-state) ) 

(do  ( (i  -1  (+  i  0.2) ) ) 

(  (>-  i  1)  ) 

(do  ((j  -1  (+  j  0.2))) 

( (>-  j  1) ) 

(setf  (aref  point -grasp- frame  0)  i) 

(setf  (aref  point -grasp-frame  1)  j) 

(setq  point-hand-frame  (transform-points-frame-global  ‘grasp-frame*  point -grasp-f ! 

tame)  ) 

(setq  constraint-state  (determine-constraint-state  point -hand- frame  force-magnitu ! 

de) ) 

(sphere-Sd-grasp-window  point -hand- frame  0.01) 

(setf  (aref  point -grasp-f rame  1)  (+  j  O.Ol)) 

(setq  point-hand-frame  (transform-points-frame-global  ‘grasp-frame*  point-grasp-f ! 

rame)  ) 

(draw-3d-list  point -hand- frame  constraint-state) ) ) ) ) 


***** 


;  9.  Hand  actuation  routines 


;  9.1  Define  actuation  constants  and  variables 

(defvar  *default-tra jectory-gen*  (if  (boundp  'tg)  tg) ) 

(defvar  ‘default-parallel-conn*  (if  (boundp  'pc)  pc)) 

(defvar  ‘fingertip-radius*) 

(defvar  ‘test-point*) 

(defvar  *scale-force*) 


;  9.3  Set  actuation  variables 

(setq  ‘fingertip-radius*  0.408) 

(setq  ‘test-point*  (ma)ce-array  '(3)  ; initial-contents  '(0.0  2.5  0.0))) 

(setq  ‘scale-force*  1.0) 

(setq  ‘offset-wrench*  (list  (zero-vector  6)  (zero-vector  6)  (zero-vector  6)  ) ) 
;  9.4  Grasp  functions 

;  9.4.1  MOVE-TO-CONTACT-POINTS 


(defun  move-to-contact-points  () 

(let*  ( (fingertip-vector  (construct-fingertip-vector 

(determine-fingertip-centers  ‘contact-points*  ‘contact-fram! 

es*) ) ) 

(dur  0.4)) 

(if  (send  tg  :move-f ingers-to  f ingertip-vector  iduration  dur) 

(send  tg  :send-traj  pc)))) 


9.4.2  MOVE-TO-GRASP -CENTER 


(defun  move-to-grasp-center  (force-center  force-magnitude) 

(let*  ( (force-center-hand-franve  (transform-points-f raroe-global  *grasp-f rame*  force-cen 
ter)  ) 

(magnitude  (*  *3cale-force*  force-magnitude)) 

(grasp-vector-liat  (conatruct-grasp-points  force-center-hand-frame  magnitude) ) 
(f  i.'gertip-center-liat 

(determine-fingertip-centera  graap-vector-liat  *contact-f ramea*) ) 
(fingertip-vector  (conatruct-fingertip-vector  fingertip-center-liat) ) 

(dur  0.4) ) 

(construct-off set-wrenchea  force-center  force-magnitude) 

(clearacreen) 

(draw-coord'.r  ace-system) 

(sphere-3d-gra3p-window  (first  grasp-vector-liat)  0.01) 

(sphere- jC-grasp-window  (second  graap-vector-liat)  0.01) 

(sphere-3d-grasp-window  (third  graap-vector-liat)  0.01) 

(3phere-3d-grasp-window  force-center-hand-frame  0.02) 

(if  (send  tg  ;move-f ingers-to  fingertip-vector  :duration  dur) 

(send  tg  :send-traj  pc)))) 


9.5  Auxiliary  functions 

9.6.1  MOVE-TO-CONTACT-FOINTS  auxiliary  function 


(defun  determine-fingertip-centera  (contact -point a  contact-frames) 

(cond  ( (null  contact-frames)  nil) 

(t  (cons  (determine-f ingertip-center  (car  contact -points)  (car  contact-frames)) 
(determine-fingertip-centera  (cdr  contact -points)  (cdr  contact-frames) ) ) 

)  )  ) 

(defun  determine-f ingertip-center  (contact -point  contact-frame) 

(let*  ((fingertip-center  (zero-vector  3))) 

(setf  (aref  fingertip-center  0) 

(+  (*  (aref  contact-frame  0  2)  ‘fingertip-radius*)  (aref  contact -point  0))) 
(setf  (aref  fingertip-center  1) 

(+  (*  (aref  contact -fr2une  1  2)  ‘fingertip- radius*)  (aref  contact -point  1))) 
(setf  (aref  fingertip-center  2) 

(+  (*  (aref  contact-frame  2  2)  *fingertip-radiu3*)  (aref  contact-point  2))) 
fingertip-center) ) 

(defun  construct-fingertip-vector  (fingertip-centers) 

(let*  ( (n-fingertips  (count-atoms  fingertip-centers)) 

(fingertip-vector  (zero-vector  (*  3  n-fingertips)))) 

(do  ( (i  0  (+  i  1)  ) ) 

( (“  i  n-fingertips)  fingertip-vector) 

(do  ( ( j  0  (+  j  1)  ) ) 

((»  j  3)) 

(setf  (aref  fingertip-vector  (+  (*  i  3)  j) )  (*  2.54  (aref  (car  fingertip-centers 

)  j) )  ) ) 

(setq  fingertip-centers  (cdr  fingertip-centers))))) 

;  9.5.2  MOVE-TO-GRASP-CENTER  auxiliary  functions 

(defun  construct-grasp-points  (force-center  force-magnitude) 

(let*  ((grasp-points  (list  (zero-vector  3)  (zero-vector  3)  (zero-vector  3))) 

(offset-points  (offset-wrench-three-contacts  force-center  force-magnitude) ) ) 
(setf  (aref  (first  grasp-points)  0)  (+  (aref  (first  ‘contact-frames*)  0  3)  (aref  (fi 

rst  of f set -points)  0))) 

(setf  (aref  (first  grasp-points)  1)  (+  (aref  (first  ‘contact-frames*)  1  3)  (aref  (fi 

rst  offset -points)  1))) 

(setf  (aref  (first  grasp-points)  2)  (+  <aref  (first  ‘contact-frames*)  2  3)  (aref  (fi 

rst  of fset -points)  2))) 


(setf  (aref  (second  grasp-points)  0)  (+  (aref  (second  *contact-fr£unes*)  0  3)  (aref  ( 
second  offset-points)  0))) 

(setf  (aref  (second  grasp-points)  1)  (+  (aref  (second  *contact-f rames*)  1  3)  (aref  ( 

second  offset-points)  1))) 

(setf  (aref  (second  grasp-points)  2)  {+  (aref  (second  ‘contact-frames* )  2  3)  (aref  ( 

second  offset-points)  2) ) ) 

(setf  (aref  (third  grasp-points)  0)  (+  (aref  (third  ‘contact-frames*)  0  3)  (aref  (th 

ird  of f set -points)  0) ) ) 

(setf  (aref  (third  grasp-points)  1)  (+  (aref  (third  ‘contact-frames*)  1  3)  (aref  (th 

ird  offset-points)  1) ) ) 

(setf  jref  (third  grasp-points)  2)  (+  (aref  (third  ‘contact-frames*)  2  3)  (aref  (th 

ird  offset-points)  2) ) ) 

grasp-points) ) 

;;A11  the  move  functions  will  return  either  NIL  or  an  integer.  NIL  is  returned  if  the 
;; trajectory  is  not  feasible.  NIL  is  also  returned  when  there's  transmission 
; /problems  in  the  parallel  connection. 


9.7  Move  load  functions 

i.e.  generates  a  trajectory  but  does  not  send  i 


;  9.7.1  Advanced  load  moves 

(defun  move-to-contact-points-load  () 

(let*  ( (fi  igert ip-vector  (construct-fingertip-vector 

(determine-f ingertip-centers  *contact-points*  *contact-f ram 

es*)  )  ) 

(dur  0.4)) 

(send  tg  :move-f ingers-to  fingertip-vector  /duration  dur) ) ) 

(defun  move-to-grasp-center-load  (force-center  force-magnitude) 

(let*  ( (force-center-hand-frame  (transform-points-f rame-global  ‘grasp-frame*  force-cen 
ter) ) 

(magnitude  (*  ‘scale-force*  force-magnitude) ) 

(grasp-vector-list  (construct-grasp-points  force-center-hand-frame  magnitude) ) 
(fingertip-center-list 

(determine-f ingertip-centers  grasp-vector-list  ‘contact-frames*) ) 
(fingertip-vector  (construct-fingertip-vector  fingertip-center-list) ) 

(dur  0.8)) 

(print -array-list  grasp- vector-list) 

(setq  *of rset-wrench*  (list  (multiply-array-list 

(list  ‘scale-force* 

(add-array-list  (list  (first  grasp-vector-list) 

(multiply-array-list 

(list  -1  (first  ‘contact-points*))) 

)  )  )  ) 

(multiply-array-list 
(list  ‘scale-force* 

(add-array-list  (list  (second  grasp-vector-list) 

(multiply-array-list 

(list  -1  (second  ‘contact -points*) ) 

)  )  )  )  ) 

(multiply-array-list 
(list  ‘scale-force* 

(add-array-list  (list  (third  grasp-vector-list) 

(multiply-array-list 

(list  -1  (third  ‘contact-points*))) 

)))))) 

(clearscreen) 

(draw-coordinate- system) 

(3phere-3d-grasp-window  (first  grasp-vector-list)  0.01) 


( sphere- 3d-grasp-window  (second  grasp-vector-list)  0.01) 

(sphere-3d-grasp-window  (third  grasp-vector-list)  0.01) 

(sphere-3d-grasp-window  force-center-hand-frame  0.02) 

(send  tg  :move-f ingers-to  fingertip-vector  :duration  dur) ) ) 

;  9.7.2  Basic  load  moves 

(defun  move-up-load  (Soptional  (dist  1.0)  4)cey  niomber-of-segs  duration 

(tra j-gen  *default-tra jectory-gen*) ) 

(let  ( (nsegs  (if  number-of-segs  number-of-segs  (send  tra j-gen  ;default-segs-per-move) ) 

) 

(dur  (if  duration  duration  (send  tra j-gen  : default-duration) )) ) 

(setf  (aref  work-transi-vect  0)  0.0 

(aref  work-transl-vect  1)  0.0 

(aref  work-transl-vect  2)  dist) 

(if  (send  traj-gen  ;generate-tra j  (list  (list  nil  nil  work-transl-vect) ) 

: number-of-segs  nsegs 
: duration  dur) 

(progn  (setf  (aref  *ob ject-f rame*  2  3)  (+  (aref  ‘object-frame*  2  3)  (/  dist  2.54 

)  ) ) 

(initialize-global-variables) ) ) ) ) 


(defun  move-down-load  (fioptional  (dist  1.0)  &key  number-of-segs  duration 

(traj-gen  *default-tra jectory-gen*) ) 

(let  ( (nsegs  (if  number-of-segs  number-of-segs  (send  traj-gen  :default-segs-per-move) ) 

) 


)  )  ) 


(dur  (if  duration  duration  (send  traj-gen  ; default-duration) )) ) 

(setf  (aref  work-transl-vect  0)  0.0 

(aref  work-transl-vect  1)  0.0 

(aref  work-transl-vect  2)  (-  dist)) 

(if  (send  traj-gen  :generate-tra j  (list  (list  nil  nil  work-transl-vect) ) 

: number-of-segs  nsegs 
; duration  dur) 

(progn  (setf  (aref  *object-f rame*  2  3)  (-  (aref  *object-f rame*  2  3)  (/  dist  2.54 

(initialize-global-variables) ) ) ) ) 


(defun  move-left-load  (ioptional  (dist  1.0)  4key  number-of-segs  duration 

(traj-gen  *default-tra jectory-gen*)  ) 

(let  ((nsegs  (if  number-of-segs  number-of-segs  (send  traj-gen  :default-segs-per-move) ) 

) 


)  ) ) 


(dur  (if  duration  duration  (send  traj-gen  :default-duration) ) ) ) 

(setf  (aref  work-transl-vect  0)  (-  dist) 

(aref  work-transl-vect  1)  0.0 

(aref  work-transl-vect  2)  0.0) 

(if  (send  traj-gen  :generate-tra j  (list  (list  nil  nil  work-transl-vect) ) 

: number-of-segs  nsegs 
;duration  dur) 

(progn  (setf  (aref  ‘object-frame*  0  3)  (-  (aref  ‘object-frame*  0  3)  (/  dist  2.54 

(initialize-global-variables) ) ) ) ) 


(defun  move-right-load  (Soptional  (dist  1.0)  &key  number-of-segs  duration 

(traj-gen  *default-tra jectory-gen*) ) 

(let  ((nsegs  (if  number-of-segs  number-of-segs  (send  traj-gen  ;default-segs-per-move)  ) 

) 

(dur  (if  duration  duration  (send  traj-gen  :def ault-duration) ) ) ) 

(setf  (aref  work-transl-vect  0)  dist 

(aref  work-transl-vect  1)  0.0 

(aref  work-transl-vect  2)  0.0) 

(if  (send  traj-gen  -.generate-tra j  (list  (list  nil  nil  work-transl-vect)) 

: number-of-segs  nsegs 
: duration  dur) 

(progn  (setf  (aref  ‘object-frame*  0  3)  (+  (aref  ‘object-frame*  0  3)  (/  dist  2.54 

)  )  ) 


(initialize-global-variables) ) ) ) ) 


(defun  move-in-load  (Soptional  (dist  1.0)  Ckey  niomber-of-segs  duration 

(traj-gen  *default-tra jectory-gen*) ) 

(let  ( (nsegs  (if  number-of-segs  number-of-segs  (send  traj-gen  ;default-segs-per-move) ) 

) 

(dur  (if  duration  duration  (send  traj-gen  : default-duration) )) ) 

(setf  (aref  work-transl-vect  0)  0.0 

(aref  wor)c-transl-vect  1)  (-  dist) 

(aref  work-transl-vect  2)  0.0) 

(if  (send  traj-gen  :generate-tra j  (list  (list  nil  nil  wor)c-transl-vect) ) 

: number-of-segs  nsegs 
: duration  dur) 

(progn  (setf  (aref  *object-frame»  1  3)  (-  (aref  *object- frame*  1  3)  (/  dist  2.54 

)  )  ) 

(initialize-global-variables) ) ) ) ) 

(defun  move-out-load  (Soptional  (dist  1.0)  Skey  number-of-segs  duration 

(traj-gen  *default-tra jectory-gen*) ) 

(let  ( (nsegs  (if  nximber-of-segs  number-of-segs  (send  traj-gen  idefault-segs-per-move)  ) 

) 

(dur  (if  duration  duration  (send  traj-gen  : default-duration) ) ) ) 

(setf  (aref  work-transl-vect  0)  0.0 

(aref  work-transl-vect  1)  dist 

(aref  work-transl-vect  2)  0.0) 

(if  (send  traj-gen  :generate-tra j  (list  (list  nil  nil  work-transl-vect) ) 

: number-of-segs  nsegs 
: duration  dur) 

(progn  (setf  (aref  *ob ject-f rame*  1  3)  (+  (aref  *object -frame*  1  3)  (/  dist  2.54 

)  )  ) 

(initialize-global-variables) ) ) ) ) 

(defun  rotate-x-load  (Soptional  (ang  10.0)  Skey  number-of-segs 

duration 

(traj-gen  *default-tra jectory-gen*) ) 

; /convert  angles  from  degrees  to  radians 
(setq  ang  (*  ang  0.01745329)) 

(let  ((nsegs  (if  number-of-segs  number-of-segs  (send  traj-gen  :default-segs-per-move) ) 

) 

(dur  (if  duration  duration  (send  traj-gen  /default-duration))) 

(temp-frame  (make-array  '(3  4)  : initial-contents  '((1  0  0  0) 

(0  10  0) 

(001  0))))) 

(setf  (aref  temp-frame  0  3)  (aref  *grasp-frame*  0  3) ) 

(setf  (aref  temp-frame  1  3)  (aref  *grasp-frame*  1  3)) 

(setf  (aref  temp-frame  2  3)  (aref  *grasp-frame*  2  3) ) 

(setq  *ob ject-f rame*  (transform-frames-global-frame  temp-frame  *ob ject-f rame*) ) 

(setf  (aref  temp-frame  0  1)  (cos  ang)) 

(setf  (aref  temp-frame  0  2)  (*  -1  (sin  ang))) 

(setf  (aref  temp-frame  1  1)  (sin  ang)) 

(setf  (aref  temp-frame  1  2)  (cos  ang)) 

(setq  *ob ject-f rame*  (transform-f rames-frame-global  temp-frame  *object-frame*) ) 
(draw-coordinates  *ob ject-f rame*  0.5) 

(if  (send  traj-gen  :generate-tra j  (list  (list  'xhat  ang  nil)) 

: number-of-segs  nsegs  /duration  dur) 

(progn  (setq  *ob ject-f rame*  (transform-f rames-frame-global  temp-frame  *object-fr 

ame  * ) ) 

(initialize-global-variables) ) ) ) ) 

(defun  rotate-y-load  (Soptional  (ang  10.0)  Skey  number-of-segs 

duration 

(traj-gen  *default-tra jectory-gen*) ) 

(setq  ang  (*  ang  0.01745329)) 

(let  ((nsegs  (if  number-of-segs  number-of-segs  (send  traj-gen  :default-segs-per-move) ) 

) 

(dur  (if  duration  duration  (send  traj-gen  /default-duration) ) ) 

(temp-frame  '(zero-array  3  4))) 


(setf  (aref  temp-frame  1  1)  1) 

(setf  (aref  temp-frame  0  0)  (cos  ang) ) 
(setf  (aref  temp-frame  02)  (*  -I  (sin 
(setf  (aref  temp-frame  2  0)  (sin  ang) ) 
(setf  (aref  temp-frame  2  2)  (cos  ang)) 
(setf  (aref  temp-frame  0  3)  (-  (aref  * 
(setf  (aref  temp-frame  1  3)  (-  (aref  * 


I  (sin  ang) ) ) 


(aref  *grasp-frame*  0  3)  (aref  *ob ject-f rame*  0  3))) 
(aref  *grasp-frame*  1  3)  (aref  *object-f rame*  13))) 


(setf  (aref  temp-frame  2  3)  (-  (aref  ‘grasp-frame*  2  3)  (aref  ‘object-frame*  23))) 

(if  (send  traj-gen  :generate-tra j  (list  (list  'yhat  ang  nil)) 

:number-of-segs  nsegs  rduration  dur) 

(progn  (setq  ‘object -frame*  (transf orm-f rames-f rame-global  teit^-frame  ‘object-fr! 

ame*) ) 

(initialize-global-variables) ) ) ) ) 

(defun  rotate-z-load  (fioptional  (ang  10.0)  filcey  number-of-segs 

duration 

(traj-gen  ‘default-trajectory-gen*) ) 

(setq  ang  (*  ang  0.01745329)) 

(let  ( (nsegs  (if  number-of-segs  number-of-segs  (send  traj-gen  ;default-segs-per-move)  )  I 

) 

(dur  (if  duration  duration  (send  traj-gen  : default-duration) ) ) 

(temp-frame  (zero-array  3  4))) 

(setf  (aref  temp-frame  2  2)  1) 

(setf  (aref  temp-frame  0  0)  (cos  ang)) 

(setf  (aref  temp-frame  0  1)  (*  -1  (sin  ang))) 

(setf  (aref  temp-frame  1  0)  (sin  ang)) 

(setf  (aref  temp-frame  1  1)  (cos  ang) ) 

(setf  (aref  temp-frame  0  3)  (-  (aref  ‘grasp-frame*  0  3)  (aref  ‘object -frame*  0  3))) 

(setf  (aref  temp-frame  1  3)  (-  (aref  ‘grasp-frame*  1  3)  (aref  ‘object -frame*  1  3) ) ) 

(setf  (aref  temp-frame  2  3)  (-  (aref  ‘grasp-frame*  2  3)  (aref  ‘object-frame*  2  3) ) ) 

(if  (send  traj-gen  :generate-tra j  (list  (list  ' zhat  ang  nil)) 

: number-of-segs  nsegs  :duration  dur) 

(progn  (setq  ‘object-frame*  (transform-f rames-f rame-global  temp-frame  *object-fr! 

ame*) ) 

(initialize-global-variables) ) ) ) ) 

(defun  move-finger-load  (finger-number  displacement -vector) 

(let  ((traj-gen  *default-tra jectory-gen*) 

(dur  0.3) 

(displacement  (zero-vector  9))) 

(setf  (aref  displacement  (+  (*  3  (-  finger-number  1))  0))  (aref  displacement-vector  ! 

0) ) 

(setf  (aref  displacement  (+  (*  3  (-  finger-nximber  1))  1))  (aref  displacement-vector  ! 

1) ) 

(setf  (aref  displacement  (+  (*  3  (-  finger-number  1) )  2) )  (aref  displacement-vector  ! 

2) ) 

(send  traj-gen  :move-f ingers-by  displacement  '.duration  dur) ) ) 

(defun  go-hand  () 

(send  tg  :send-traj  pc)) 

;  9.8  Basic  moves 


(defun  up  (Soptional  (distance  2.0)) 

(if  (move-up-load  distance) 

(progn  (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 

(defun  down  (Aoptional  (distance  2.0)) 
(if  (move-down-load  distance) 

(progn  (clearscreen) 

(draw-coordinate-system) 
(go-hand) ) ) ) 

(defun  right  (ioptional  (distance  2.0)) 
(if  (move-right-load  distance) 

(progn  (clearscreen) 


(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  left  (^optional  (distance  2.0)) 

(if  (move-left -load  distance) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  in  (fioptional  (distance  2.0)) 

(if  (move-in-load  distance) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  out  (fioptional  (distance  2.0)) 

(if  (move-out-load  distance) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  rot+x  (ioptional  (angle  10.0)) 

(if  (rotate-x-load  angle) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  rot+y  (ioptional  (angle  10.0)) 

(if  (rotate-y-load  angle) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  rot+2  (Soptional  (angle  10.0)) 

(if  (rotate-2-load  angle) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  rot-x  (Soptional  (angle  -10.0)) 

(if  (rotate-x-load  angle) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  rot-y  (Soptional  (angle  -10.0)) 

(if  (rotate-y-load  angle) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

(defun  rot-2  (Soptional  (angle  -10.0)) 

(if  (rotate-2-load  angle) 

(progn  (clearscreen) 

(draw-coordinate-system) 

(go-hand) ) ) ) 

.;.**«***llr******«*****«*****«***************«*****'****«************'****************«*1^**t 


;  10,  Menu 

...«**«*i^*****************************************************««***ilr***************«****! 
*  «  *  * 

;  10.1  Define  menu  variables 

(defvar  menu) 

(defvar  moves-menu) 

(defvar  dem-menu) 

;  10.2  Define  menu 

(setq  menu  (tv:ma)ce-window  ' tv;momentary-menu 

':label  '(:string  #.(2l:string  "Grasp  Menu:  ") ) 

' : geometry  (list  2) 


' : borders  3 

':item-list  '(("STANDARD  GRAPHICS  OPTIONS:  "  :no-select) 

("STANDARD  MOTION  OPTIONS:  "  :no-select) 

("Create  grasp  screen  "  :£uncall  make-grasp-screen) 

("Reinit  system  "  :£uncall  reinit) 

("Clear  screen"  :£uncall  clearscreen) 

("Reinit  OOLAH  trajectory  "  :£uncall  init-oolah) 

("Draw  coordinate  system  "  :£uncall  draw-coordinate-system) 
("Reinit  VAX  trajectory  "  :£uncall  init-vax) 

("Change  graphics  variables"  :£uncall  change-graphics-variables-me 

nu) 

("Go  home  "  :£uncall  go-horoe-and-init) 

(""  ;no-select) 

("BASIC  MOVES  "  :£uncall  basic-moves-menu) 

(""  : no-select) 

("Move  finger  "  :£uncall  move-finger-menu) 

(""  : no-select) 

(""  :no-select) 

("SLIP  ANALYSIS  OPTIONS:  "  :no-select) 

("ADVANCED  HAND  ACTUATION  OPTIONS:  "  :no-select) 

("Change  global  variables  "  :£uncall  change-global-variables-menu) 
("JKS  move  menu  "  :£uncall  move-menu) 

("Permissible-twist  "  :£uncall  permissible-twist-menu) 

("Move  to  contact  points"  :£uncall  move-to-contact -points) 
("Determine  constraint  state  "  :funcall  determine-con'^traint-state 

-menu) 

("Pic)c  grasp  force  center  "  :funcall  grasp-force-center-menu) 

("Map  constraint  space  "  :funcall  map-constraint-state-menu) 
("Controlled  slip  "  ;f uncall  controlled-slip-menu) 

(""  :no-select) 

("DEMONSTRATIONS"  :funcall  demo-menu) 

(""  : no-select) 

(""  :no-select) 

(""  :no-select) 

("QUIT"  :eval  999)))) 

;  10.3  Standard  graphics  options 

;  10.3.1  MAKE-GRASP-SCREEN 

;  10.3.2  CLEARSCREEN 

;  10.3.3  DRAW-COORDINATE-SYSTEM 

(defun  draw-coordinate-system  () 

(setq  view-frame  (construct-view-frame  angle-x  angle-z) ) 

(draw-coordinates  "hand-frame*  1) 

(draw-coordinates  ‘object-frame*  0.75) 

(draw-coordinates  *grasp-f rame*  0.5) 

(draw-coordinates  "contact-frames*  0.2)) 

;  10.3.4  CHANGE-GRAPHICS-VARIABLES-MENU 


(defun  change-graphics-variables-menu  () 

(let  ( (zl-user : scale-3d  scale-3d) 

( zl-user : x-origin-3d  x-origin-3d) 

(zl-user : y-origin-3d  y-origin-3d) 

(zl-user : angle-x  angle-x) 

(zl-user : angle-z  angle-z)) 

(zl-user :choose-user-options  zl-user :giraphics-variables-menu) 
(setq  scale-3d  zl-user :scale-3d) 

(setq  x-origin-3d  zl-user :x-origin-3d) 

(setq  y-origin-3d  zl-user :y-origin-3d) 

(setq  angle-x  zl-user : angle-x) 

(setq  angle-z  zl-user :angle-z) ) ) 


10.4  Standard  motion  options 
10.4.0  REINIT 


;  10.4.1  INIT-OOLAH 

(defun  init-oolah  () 

(send  tg  :init)) 

;  10.4.2  INIT-VAX 

(defun  init-vax  () 

(send  tg  :init-traj  pc)) 

;  10.4.3  GO-HOME-AND-INIT 

(defun  go-home-and-init  () 

(set -original -values) 

(setq  *ob ject-f rame*  *original-ob ject-f rame*) 

(setq  *contact-point3-object -space*  *original-contact -points -object -space*) 
(setq  *contact-nonnal3-pbject -space*  *original-cont act -normals -object -space*) 
(initialize-global-variables) 

(init-oolah) 

(init-vax) 

(go-home) 

(init-vax) 

(go -home) 

(clearscreen) 

(draw-coordinate-system) ) 


10.4.4  BASIC-MOVES 


(defun  basic-moves-menu  () 

(do  (  (i  0  (+  i  1)  ) ) 

( (equalp  (send  moves-menu  choose)  999)  'Done))) 


(setq  moves-menu  (tv;ma)ce-window  ' tv. "momentary-menu 

label  '(:string  #.(zl:string  "Basic  moves  menu:  ")) 
'  ."geometry  (list  3) 

' : borders  2 

':item-list  '(("OUT"  :funcall  out) 

("UP"  :f uncall  up) 

(""  : no-select) 

("LEFT"  :funcall  left) 

("QUIT"  :eval  999) 

("RIGHT"  :f uncall  right) 

(""  : no-select) 

("DOWN"  :f uncall  down) 

("IN"  :f uncall  in) 

(""  ."no-select) 

(""  : no-select) 

(""  :no-select) 

("ROT-X  "  : no-select) 

("ROT-y  "  : no-select) 

("ROT-Z  "  : no-select) 


(" 

+ 

tl 

: funcall 

rot"^x) 

(" 

+ 

tf 

:  funcall 

rot■^y) 

(" 

+ 

«1 

:  funcall 

rot■^z) 

(" 

- 

ft 

:  funcall 

rot-x) 

(" 

- 

ft 

: funcall 

rot-y) 

(" 

- 

ft 

: funcall 

rot-z) ) ) ) 

'■ 

10.4.5  MOVE-FINGER-MENU 

(defun  move-finger-menu 

0 

(let  ((displacement  (ma)te-array  '(3)))) 

(zl-user : choose-user-options  zl-user : finger-menu) 
(setf  (aref  displacement  0)  zl-user ;x-di3placement) 
(setf  (aref  displacement  1)  zl-user :y-displacement) 
(setf  (aref  displacement  2)  zl-user : z-displacement) 
(move-finger-load  zl-user : finger-number  displacement) 
(go-hand) ) ) 


10.5  SLIP-ANALYSIS 


;  10.5.1  CHANGE-GLOBAL-VARIABLES-MENU 

(defvar  variables-menu) 

(defun  change-global-variables-menu  () 

(do  ((i  0  (+  i  1))) 

( (equalp  (send  variables-menu  choose)  999)  'Done))) 

(setq  variables-menu  (tv: make-window  'tv:momentary-menu 

label  '(istring  #.(zl:string  "Variables  moves  menu:  ")) 

' : geometry  (list  1) 

' : borders  2 

': item-list  '(("Object  variables"  rfuncall  object -variable-menu) 

("Contact  variables"  :funcall  contact-variable-menu) 
("Body  wrench"  :f uncall  body- wrench-menu) 

("Finger  variables"  : no-select) 

(""  : no-select) 

("QUIT"  :eval  999)))) 


(defun  object-variable-menu  () 

(let  ( (zl-user: "object-frame*  "object-frame")) 

(zl-user: object-variable-menu) 

(setq  "object-frame"  zl-user: "object-frame") 

(initialize-global-variables) )  ) 

(defun  contact-variable-menu  () 

(let  ( (zl-user : "contact -points-object-space"  "contact-points-object-space") 

(zl-user : *contact-normals-ob ject-space*  "contact-normals-ob ject-space") ) 
(zl-user : contact -variable-menu) 

(setq  "contact-points-object-space"  zl-user : "contact-points-object-space") 
(setq  "contact-normals-ob ject-space*  zl-user: "contact-normals-object-space") 
(initialize-global-variables) ) ) 


(defun  body-wrench-menu ( ) 

(setq  zl-user : "body-wrench"  "body-wrench") 

(zl-user : body- wrench-menu) 

isetq  "body-wrench*  zl-user : "body-wrench") ) 

;  10.5.2  PERMISSIBLE-TWIST-MENU 

(defun  permissible-twist-menu  () 

(let  ((twist  (make-array  '(6)))) 

(zl-user : permissible-twist -menu) 

(setq  twist  zl-user : "twist") 

(permissible-twist  twist) ) ) 


10.5.3  DETERMINE-CONSTRAINT-STATE-MENU 


(defun  determine-constraint-state-menu  () 

(draw-coordinate-system) 

(let  ((magnitude  0) 

(force-center  (zero-vector  3)) 

(force-center-hand-frame  (zero-vector  3)>) 

(zl-user :choose-user-options  zl-user : constraint-state-menu) 

(setq  magnitude  zl-user ;magnitude) 

(setq  force-center  (get-mouse-coordinates-grasp-f rame) ) 

(setq  force-center-hand-frame  (transform-points-f rame-global  "grasp-frame*  force-cen 
ter) ) 

(determine-constraint-state  force-center-hand-frame  magnitude) ) ) 

;  10.5.4  MAP-CONSTRAINT-STATE-MENU 


(defun  map-constraint-state-menu  () 


(draw-coordinate-system) 

(zl-uaer :choose-user-optiona  zl-uaer traap-atate-menu) 

(let  ( (x-inc  (/  (-  zl-uaer :x-end  zl-uaer :x-atart)  zl-uaer :x-atepa) ) 
(y-inc  (/  (-  zl-uaer :y-end  zl-uaer ;y-atart)  zl-uaer :y-atepa) ) ) 
(map-conatraint-atate  zl-uaer :x-3tart  zl-uaer :x-end  x-inc 

zl-uaer :y-atart  zl-uaer :y-end  y-inc 
zl-uaer tmagnitude) ) ) 


;  10.6  ADVANCED-HAND-ACTUATION-OPTIONS 

;  10.6.1  JKS  MOVE-MENU 

;  10.6.2  MOVE-TO-CONTACT-POINTS 

;  10.6.3  GRASP-FORCE -CENTER-MENU 

(defun  graap-force-center-menu  () 

(let  ( (magnitude  0) 

(force-center  (zero-vector  3))) 

( zl-user : chooae-uaer-optiona  zl-uaer : conatraint-atate-menu) 

(aetq  magnitude  zl-uaer :magnitude) 

(aetq  force-center  (get-mouae-coordinatea-graap-f rame) ) 

(move-to-graap-center  force-center  magnitude) ) ) 

;  10.6.3.1  GRASP -FORCE-CENTER-MENU  Auxiliary  func 

tiona 

(defun  get-mouae-coordinatea-graap-f rame  () 

(let  ( (mouae-coordinatea-view-f rame  (get -mouae-coordinatea-view-f rame) ) ) 
(tranaform-normala-view-graap-frame  mouae-coordinatea-view-f rame) ) ) 


(defun  get-mouae-coordinatea-view-f rame  () 

(let  ( (mouae-coordinatea  (get-mouae-coordinatea) ) 

(mc-view-f rame  (zero-vector  3)) 

(window-height  (aend  graap-acreen  :aend-pane  ' graap-window  height) ) ) 

(aetf  (aref  mc-view-f rame  0)  (/  (-  (firat  mouae-coordinatea)  x-origin-3d)  scale-3d) ) 
(aetf  (aref  mc-view-f rame  2)  (/  (-  window-height  (+  (aecond  mouae-coordinatea)  y-ori 

gin-3d) ) 

8cale-3d) ) 

mc-view-f rame) ) 

(defun  get-mouae-coordinatea  () 

(let  ( (mouae-coordinatea  (cdr  (multiple-value-liat  (tv; with-mouae-and-buttona-grabbed 

(tv: wait-for-mouae-button-down) ) ) 

) ) ) 

mouae-coordinatea) ) 

(defun  transform-normala-view-graap-frame  (point-view-frame) 

(let*  ( (graap-to-view-frame  (conatruct-graap-to-view-f rame) ) 

(all  (aref  graap-to-view-frame  0  0) ) 

(al2  (aref  graap-to-view-frame  0  1)) 

(a31  (aref  graap-to-view-frame  2  0)) 

(a32  (aref  graap-to-view-frame  2  1)) 

(al4  (aref  graap-to-view-frame  0  3)) 

(a34  (aref  graap-to-view-frame  23)) 

(determinant  (-  (*  all  a32)  (*  al2  a31))) 

(XV  (aref  point-view-f raune  0)) 

(zv  (aref  point-view-frame  2) ) 

(point-graap-frame  (zero-vector  3))) 

(aetf  (aref  point-graap-frame  0)  (/  (+  (*  a32  (-  xv  al4)) 

(*  -1  al2  (-  zv  a34)))  determinant)) 

(aetf  (aref  point-graap-frame  1)  (/  (+  (*  -1  a31  (-  xv  al4) ) 

(*  all  (-  zv  a34)))  determinant)) 

point-graap-frame) ) 

(defun  conatruct-graap-to-view-f rame  () 

(let  ((graap-to-view-frame  (zero-array  3  4))) 


(do  ( (i  0  (+  i  1) ) ) 

((-  i  3)) 

(do  ((j  0  (+  j  1))) 

((-  j  3)) 

(do  ( (k  0  (+  k  1) ) ) 

((-  k  3)) 

(setf  (aref  grasp-to-view-f rame  i  j)  (+  (*  (aref  *grasp-f rame*  k  j) 

(are£  view-frame  k  i) ) 

(aref  grasp-to-view-f rame  i  j)))))) 

(do  ((i  0  (+  i  1))) 

((-  i  3)) 

(do  ((j  0  (+  j  1))) 

((-  j  3)) 

(setf  (aref  grasp-to-view-frame  i  3)  (+  (*  (aref  view-frame  j  i) 

(-  (aref  *grasp-f rame*  j  3) 

(aref  view-freune  j  3) ) ) 

(aref  grasp-to-view-frame  i  3) ) ) ) ) 

(setf  (aref  grasp-to-view-frame  0  2)  0) 

(setf  (aref  grasp-to-view-frame  1  0)  0) 

(setf  (aref  grasp-to-view-frame  1  1)  0) 

(setf  (aref  grasp-to-view-frame  1  2)  0) 

(setf  (aref  grasp-to-view-frame  1  3)  0) 

(setf  (aref  grasp-to-view-frame  2  2)  0) 
grasp-to-view-frame) ) 

;  10.6.4  DEMO-MENU 

(defun  demo-menu  () 

(do  ( (i  0  (+  i  1) ) ) 

( (equalp  (send  dem-menu  choose)  999)  'Done))) 

;  10.7  GRASP -MENU 

{ defun  grasp-menu  () 

(do  ((i  0  (+  i  1))) 

((equalp  (send  menu  choose)  999)  'Done))) 


;  11.  Demos 

■k  -kit  It  It 


11.1  DEMO-MENU 


(defvar  dem-menu) 


(setq  dem-menu  (tv; make-window  ' tv.momentary-menu 

'•.label  '(:string  #.(zl:string  "Advanced  dexterity  demonstrations; 
' rgeometry  (list  1) 

' : borders  2 

': item-list  '(("Can  demo"  rfuncall  dlb-can-demo ) 

("Can  twirl  demo"  ;funcall  can-twirl) 

("Can  spin  demo"  rfuncall  can-spin) 

("Box  demo"  ;f uncall  box-spin) 

(hquit"  ;eval  999)))) 


")  ) 


11.2  Demonstration  programs 
11.2.0  DLB-CAN-DEMO 


(defun  dlb-can-demo  () 

(print  "You  have  3  seconds  to  place  the  can") 


(go-home-and-init) 

(sleep  3) 

(grab  4.2) 

(sleep  2) 

(move-in-load  5  : duration  0.8) 

(move-out-load  3  : duration  0.8) 

(move-left-load  5  :duration  0.6) 

(move-right-load  10  :duration  0.8) 

(move-left-lo. 5  :duration  0.6) 

(move-up-load  4  .’duration  0.6) 

(move-down-load  6  ’.duration  1.0) 

(move-up-load  2  ’.duration  0.6) 

(rotate-x-load  10  : duration  0.8) 

(rotate-x-load  -10  : duration  0.8) 

(rotate-y-load  30  : duration  0.8) 

(rotate-y-load  -60  :duration  1.6) 

(rotate-y-load  30  rduration  0.8) 

(rotate-z-load  40  :duration  0.8) 

(rotate-2-load  -80  :duration  1.6) 

(rotate-z-load  40  :duration  0.8) 

(go-hand) 

(sleep  15) 

(print  "Finished  absolute  moves") 

(set-cube-f rame) 

(move-left-load  5  :duration  0.6) 

(move-right-load  1C  ; duration  0.8) 

(move-left-load  5  :duration  0.6) 

(move-up-load  4  :duration  0.6) 

(move-down-load  6  : duration  1.0) 

(move-up-load  2  :duration  0.6) 

(rotate-x-load  20  :duration  0.8) 

(rotate-x-load  -40  :duration  0.8) 

(rotate-x-load  20  zduration  0.8) 

(rotate-y-load  40  zduration  0.8) 

(rotate-y-load  -80  zduration  1.6) 

(rotate-y-load  40  zduration  0.8) 

(rotate-z-load  40  zduration  0.8) 

(rotate-z-load  -80  zduration  1.6) 

(rotate-z-load  40  zduration  0.8) 

(sleep  15) 

(go-hand) 

(move-up-load  2  zduration  0.6) 

(move-left-load  2  zduration  0.6) 

(move-down- load  4  zduration  1.2) 

(move-right-load  4  zduration  1.2) 

(move-up-load  4  .’duration  1.2) 

(move-left-load  2  zduration  0.6) 

(move-down-load  2  zduration  0.6) 

(sleep  6) 

(go-hand) 

(send  tg  zback-to-basic) ) 

;  11.2.1  TWIRL-CAN 

(defun  can-twirl  () 

(print  "You  have  3  seconds  to  place  the  can") 

(go-home) 


(sleep  3) 

(setq  •object-frame*  (make-array  '(3  4)  : initial-contents  '((1  0  0  0) 

(0  0  -1  5.5) 
(010  -2.7)))) 

(setq  •contact -points -object -space* 

(list  (make-array  '(3)  .'initial-contents  '(-0.8  1.3  2.5)) 

(make-array  '(3)  : initial-contents  '(0.0  1.3  2.5)) 

(make-array  '(3)  : initial-contents  '(0  -1.3  2.5)))) 
(initialize-global- variables) 

(move-to-contact -points) ) 

(sleep  1) 

(let*  ((force-magnitude  1.0) 

(force-center  (zero-vector  3))) 

(move-to-grasp-center-load  force-center  force-magnitude) 

(move-right -load  4  :duration  0.6) 

(move-left -load  8  :duration  1.0) 

(move-right -load  4  .'duration  0,6) 

(move-down-load  2  rduration  0.6) 

(move-up-load  4  .'duration  1.0) 

(move-down-load  2  :duration  0.6) 

(move-to-grasp-center-load  force-center  force-magnitude) 

(setq  force-magnitude  0.2) 

(setf  (aref  force-center  0)  0.9) 

(setf  (aref  force-center  1)  0) 

(move-to-grasp-center-load  force-center  force-magnitude) 


(setq  force-magnitude  1.2) 

(setf  (aref  force-center  0)  0) 

(move-to-grasp-center-load  force-center  force-magnitude) 
(move-right -load  4  ; duration  0.6) 

(move-left-load  8  .'duration  1.0) 

(move-right -load  4  :duration  0.6) 

(move-down-load  2  ;duration  0.6) 

(move-up-load  4  : duration  0.8) 

(move-down-load  2  rduration  1,0) 

(move-to-grasp-center-load  force-center  force-magnitude) 
(go-hand) ) ) 


11.2.2  CAN-CRAWL 


;  11.2.2  CAN-SPIN 

(defun  can-spin  () 

(print  "You  have  2  seconds  to  place  the  can") 

(go-home) 

(sleep  2) 

(setq  *c/b ject-f ramq*  (make-array  '  (3  4)  rinitial-contents 


((1000) 

(0  0  -1  5.5) 

(0  1  0  -2.5) )) ) 


(setq  *cont act -points -object -space* 

(list  (make-array  '(3)  rinitial-contents  '(-0.8  1.3  2.5)) 
(make-array  '(3)  rinitial-contents  '(0.8  1.3  2.5)) 
(make-array  '(3)  rinitial-contents  '(0  -1.3  2.5)))) 
(initialize-global-variables) 

(move-to-contact -points) 

(sleep  2) 

(let*  ((force-magnitude  0.3) 

(force-center  (zero-vector  3)) 

(displacement  (zero-vector  3))) 
(move-to-grasp-center-load  force-center  force-magnitude) 

f 


(setq  force -magnitude  0.4) 

(setf  (aref  force-center  0)  -0.6) 

(move-to-grasp-center-load  force-center  force-magnitude) 


(setf  (aref  displacement  0)  0) 


(setf  (are£  displacement  1)  1) 

(set£  (aref  displacement  2)  1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  2) 

(setf  (aref  displacement  2)  -2) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  -4) 

(setf  (aref  displacement  2)  -1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  1) 

(setf  (aref  displacement  1)  1) 

(setf  (aref  displacement  2)  2) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  -0.5) 
(setf  (aref  displacement  1)  3) 
(setf  (aref  displacement  2)  -2) 
(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  -0.5) 
(setf  (aref  displacement  1)  -4.5) 
(setf  (aref  displacement  2)  -1) 
(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0.5) 
(setf  (aref  displacement  1)  1.5) 
(setf  (aref  displacement  2)  3) 
(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 
(setf  (aref  displacement  1)  3) 
(setf  (aref  displacement  2)  1) 
(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  -0.5) 
(setf  (aref  displacement  1)  1.5) 
(setf  (aref  displacement  2)  -0.5) 
(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 
(setf  (aref  displacement  1)  -4) 
(setf  (aref  displacement  2)  -0.5) 
(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  1) 
(setf  (aref  displacement  1)  1) 
(setf  (aref  displacement  2)  1) 
(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 
(setf  (aref  displacement  1)  2) 
(setf  (aref  displacement  2)  1) 
(move-finger-load  2  displacement) 

(setf  (aref  disp'' acement  0)  0) 
(setf  (aref  displacement  1)  0.5) 
(setf  (aref  displacement  2)  -2) 
(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 
(setf  (aref  displacement  1)  -2.2) 


(setf  (aref  displacement  2)  0) 
(move-finger-load  2  displacement) 


(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  0) 

(setf  (aref  displacement  2)  1) 

(move-finger-load  2  displacement) 

(setq  force-magnitude  0.5) 

(setf  (aref  force-center  0)  -0.6) 

(move-to-grasp-center-load  force-center  force-magnitude) 

(setq  force-magnitude  0.5) 

(setf  (aref  force-center  0)  0) 

(move-to-grasp-center-load  force-center  force-magnitude) 
(go-hand) ) ) 


11.2.3  BOX-SPIN 


(defun  box-spin  () 

(print  "You  have  3  seconds  to  place  the  box") 

(go-home) 

(sleep  3) 

(setq  *ob ject-f rame*  (make-array  '(3  4)  : initial-contents  '((1  0  0  1.5) 

(010  3.0) 
(001  -2.21) ) ) 

(setq  *contact-points-ob ject-space* 

(list  (m^ke-array  '(3)  ; initial-contents  '(-0.8  0.0  0.8)) 

(make-array  '(3)  : initial-contents  '(0.8  0.0  0.8)) 

(make-array  '(3)  .-initial-contents  '(0.8  0.0  -0.8)))) 

( initialize-global-variables) 

(move-to-contact-points-load) 

(move-left-load  7.6) 

(go-hand) 

(clearscreen) 

(draw-coordinate-system) 

(let*  ((force-magnitude  0.05) 

(force-center  (zero-vector  3) ) 

(displacement  (zero-vector  3))) 

(initialize-global-variables) 

(setf  (aref  force-center  0)  0.8) 

(do  (  (i  0  (+  i  1) ) ) 

( (-  i  3) ) 

(setq  force-magnitude  0.2) 

(setf  (aref  force-center  0)  0.8) 

(move-to-grasp-center-load  force-center  force-magnitude) 

;  Bring  the  finger "back 

(setf  (aref  displacement  C)  1) 

(setf  (aref  displacement  1)  0) 

(setf  (aref  displacement  2)  2) 

(move-finger-load  1  displacement) 

(setf  (aref  displacement  0)  1.13) 

(setf  (aref  displacement  1)  -1.13) 

(setf  (aref  displacement  2)  0) 

;  Push  in  and  rotate 

(move-finger-load  1  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  0) 

(setf  (aref  displacement  2)  -2.5) 

(move-finger-load  1  displacement) 

(setf  (aref  displacement  0)  -1.13) 


(set£  (are£  displacement  1)  1.13) 

(setf  (aref  displacement  2)  0) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  1.13) 

(setf  (aref  displacement  1)  1.13) 

(setf  (aref  displacement  2)  0) 

Bring  the  finger  back 
(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  0) 

(setf  (aref  displacement  2)  2.5) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  -1.13) 

(setf  (aref  displacement  1)  -1.13) 

(setf  (aref  displacement  2)  0) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  1.13) 

(setf  (aref  displacement  1)  -1.13) 

(setf  (aref  displacement  2)  0) 

Push  in  and  rotate 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  0) 

(setf  (aref  displacement  2)  -2.5) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  -1.13) 

(setf  (aref  displacement  1)  1.13) 

(setf  (aref  displacement  2)  0) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  1.13) 

(setf  (aref  displacement  1)  1.13) 

(setf  (aref  displacement  2)  0) 

(move-finger-load  1  displacement) 

Bring  the  finger  back 
(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  0) 

(setf  (aref  displacement  2)  2.5) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  -1.13) 

(setf  (aref  displacement  1)  -1.13) 

(setf  (aref  displacement  2)  0) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  1.13) 

(setf  (aref  displacement  1)  -1.13) 

(setf  (aref  displacement  2)  0) 

Push  in  and  rotate 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  0) 

(setf  (aref  displacement  2)  -2.5) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  -1.13) 

(setf  (aref  displacement  1)  1.13) 

(setf  (aref  displacement  2)  0) 

(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  1.13) 

(setf  (aref  displacement  1)  1.13) 

(setf  (aref  displacement  2)  0) 

Bring  the  finger  back 
(move-finger-load  1  displacement) 
(setf  (aref  displacement  0)  0) 


(setf  (aref  displacement  1)  1) 

(setf  (aref  displacement  2)  3) 

(move-finger-load  1  displacement) 

(setf  (aref  displacement  0)  -1.13) 

(setf  (aref  displacement  1)  -1.13) 

(setf  (aref  displacement  2)  0) 

(move -finger- load  1  displacement) 

(setq  force-magnitude  0.0) 

(setf  (aref  force-center  0)  0) 

(move-to-grasp-center-load  force-center  force-magnitude) 
(move-right-load  7.6) 

(setq  force-magnitude  0.3) 

(setf  (aref  force-center  0)  -0.5) 

(move-to-grasp-center-load  force-center  force-magnitude) 
(move-left-load  7.6)) 

(go-hand) ) ) 


(defun  back-rub  0 

(print  "You  have  2  seconds  to  place  the  back") 

(go -home) 

(sleep  2) 

(setq  *ob ject-f rarne*  (make-array  ' (3  4)  ; initial-contents  ' ((1  0  0  0) 

(0  0  -1  5.5) 

(0  1  0  -2.5) )) ) 

(setq  *cont act -point 3 -object -space* 

(list  (make-array  '(3)  : initial-contents  '(-0.8  1.3  2.5)) 

(make-array  '(3)  -.initial-contents  '(0.8  1.3  2.5)) 

(make-array  '(3)  : initial-contents  '(0  -1.3  2.5)))) 
(initialize-global-variables) 

(move-to-contact-points) 

(go-hand) ) 


(sleep  2) 

(let*  ((force-magnitude  0.3) 

(force-center  (zero-vector  3)) 

(displacement  (zero-vector  3) ) ) 
(move-to-grasp-center-load  force-center  force-magnitude) 

(setq  force-magnitude  0.4) 

(setf  (aref  force-center  0)  -0.6) 

(move-to-grasp-center-load  force-center  force-magnitude) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  1) 

(setf  (aref  displacement  2)  1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  2) 

(setf  (aref  displacement  2)  -2) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  -4) 

(setf  (aref  displacement  2)  -1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  1) 

(setf  (aref  displacement  1)  1) 

(setf  (aref  displacement  2)  2) 

(move-finger-load  2  displacement) 


(aetf  (aref  displacement  0)  -1) 

(setf  (aref  displacement  1)  3) 

(setf  (aref  displacement  2)  -2) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  -0.5) 

(setf  (aref  displacement  1)  -4.5) 

(setf  (aref  displacement  2)  -1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0.5) 

(setf  (aref  displacement  1)  1.5) 

(setf  (aref  displacement  2)  3) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  3) 

(setf  (aref  displacement  2)  1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  -1) 

(setf  (aref  displacement  1)  1.5) 

(setf  (aref  displacement  2)  -0.5) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  -5) 

(setf  (aref  displacement  2)  -1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  1) 

(setf  (aref  displacement  1)  1) 

(setf  (aref  displacement  2)  1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  2) 

(setf  (aref  displacement  2)  1) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  0.5) 

(setf  (aref  displacement  2)  -2) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  -2.5) 

(setf  (aref  displacement  2)  0) 

(move-finger-load  2  displacement) 

(setf  (aref  displacement  0)  0) 

(setf  (aref  displacement  1)  0) 

(setf  (aref  displacement  2)  1) 

(move-finger-load  2  displacement) 

(setq  force-magnitude  0.5) 

(setf  (aref  force-center  0)  -0.6) 

(move-to-grasp-center-load  force-center  force-magnitude) 

(setq  force-magnitude  0.5) 

(setf  (aref  force-center  0)  0) 

(move-to-grasp-center-load  force-center  force-magnitude) 
(go-hand) ) ) 


(defun  pic  () 


(sleep  10) 

(let  ((displacement  (ma)ce-array  '(3)))) 

(setf  (are£  displacement  0)  0.0) 

(setf  (aref  displacement  1)  0.0) 

(setf  (aref  displacement  2)  -2.5  ) 

(move-finger-load  zl-user; finger-number  displacement) 
(go-hand) 

(setf  (aref  displacement  0)  0.0) 

(setf  (aref  displacement  1)  0.0) 

(setf  (aref  displacement  2)  2.5  ) 

(move-finger-load  zl-user : finger-number  displacement) 
(go-hand) ) ) 


